【TypeScript】最近傍探索 KDTreeの球面座標系への拡張

# KDTree
k次元ユークリッド空間中の点を分割するデータ構造であり、近傍探索に非常に有効です。例えば3次元空間の $(x,y,z)$ 座標でよく利用されます。

何とかこのKDTreeのデータ構造を上手くして球面座標系$(\lambda, \phi)$=(経度,緯度)でも利用できないか？しかし緯度・経度の座標系は直交していない、単純な緯度・経度の値でユークリッド距離を計算できないため単純ではありません。

平面 $(x,y)$ での話やKDTreeによる最近傍探索アルゴリズムは別の記事にまとめましたので適宜参照してください。  
[【TypeScript】KDTreeで最近傍探索](https://qiita.com/Seo-4d696b75/items/9d0d19e17b6395f15b14)

以下、平面の話をするので$k=2$に限定します。

# 球面の座標系

半径1の単位球で考えます。角度の単位はラジアンなので実装時は適宜変換します。

```math
 \begin{cases} 
 \text{半径} :& 1 \\
 \text{経度} :& -\phi \le \lambda  \le \phi \\
 \text{緯度} :& -\frac{\pi}{2} \le \phi \le \frac{\pi}{2}
 \end{cases}
```

球面上での距離は[大円距離（測地線の長さ）](https://ja.wikipedia.org/wiki/%E5%A4%A7%E5%86%86%E8%B7%9D%E9%9B%A2)で計算します。北極と南極を結ぶ経線は直線（測地線）ですが、緯線は赤道を除き直線ではありません。

# 球面への拡張

経度・緯度をx,yに見立てれば球面でも同様に木構造はできますが探索はどうする？

```ts
interface PointGeo {
  lat: number // 緯度（オイラー角 [-90,90]）
  lng: number // 経度（オイラー角 [-180,180]）
} 

interface SearchNode extends PointGeo {
  depth: number
  left?: SearchNode
  right?: SearchNode
}
```

## 探索アルゴリズム
[探索アルゴリズムの詳細](https://qiita.com/Seo-4d696b75/items/9d0d19e17b6395f15b14#kdtree%E3%82%92%E7%94%A8%E3%81%84%E3%81%9F%E8%BF%91%E5%82%8D%E6%8E%A2%E7%B4%A2%E3%82%A2%E3%83%AB%E3%82%B4%E3%83%AA%E3%82%BA%E3%83%A0)

問題となる「２つの子ノードのうち最初に探索しなかった方に存在するかもしれない点との距離の最小値」をどう計算するか考えます。基本的には境界線上の点との距離を調べれば十分ですが、今回の境界線は緯線と経線となります。緯線・経線との距離はどう計算する？

### 緯線との距離
点 $(\lambda_0,\phi_0)$　と緯線 $\phi = \phi_1$ の距離は緯線に下した垂線の長さになりますが、これは経線の一部なので簡単。

```math
 d_{\rm min} = | \phi_0 - \phi_1|
```

### 経線との距離
点 $\rm{A} (\lambda_0,\phi_0)$　と経線 $\lambda = \lambda_1$ の距離も同様に垂線で考えますが、次の場合分けが発生します（図は上から北極点Bを中心に見たもの）。  
ただし、$|\lambda_1 - \lambda_0| \pm 2n\pi$ の違いは無視します。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/440643/0ff8f32f-d506-1002-b3cf-347c60bfee91.png" width="300">
図の<font color="red">赤色の垂線</font>の長さ$b$を求めます。三角形ABCについて[球面三角法](https://ja.wikipedia.org/wiki/%E7%90%83%E9%9D%A2%E4%B8%89%E8%A7%92%E6%B3%95)によれば、  

```math
\begin{eqnarray}
  {\rm sin} b &=& {\rm sin}B \ {\rm sin}c\\
 &=& {\rm sin}|\lambda_1 - \lambda_0| \ {\rm sin}(\frac{\phi}{2} - \phi_0) \\
 &=& {\rm sin}|\lambda_1 - \lambda_0| \ {\rm cos}\phi_0 \\
 d_{\rm min} = b &=& {\rm arcsin}\left({\rm sin}|\lambda_1 - \lambda_0| \ {\rm cos}\phi_0 \right)
\end{eqnarray}
```
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/440643/cb6cbe9a-0075-4007-086e-536e0a907345.png" width="300">
<font color="red">垂線</font>の様子がだいぶ違います。垂線の足Cが反対側に位置しており、<font color="blue">極との距離</font>（図では北極との距離）の方が短くなります。

```math
d_{\rm min} = {\rm min}\{ \ | \pm \frac{\pi}{2} - \phi_0 | \ \}
```


## 実装

緯線・経線との距離の計算方法が分かったので実装。
ここでは端点も考慮して存在するかもしれない点との距離最小値を計算しています。

```ts:dist.ts
interface Region {
  north: number
  south: number
  west: number
  east: number
}

/**
 * ある点と緯線・経線で囲まれた領域中の点との距離の最小値を計算する
 * 
 * @param pos regionの内部にはないこと
 * @param region 
 * @param node 現在の探索ノード
 * @returns 領域中の点との距離の最小値
 */
function minDist2Region(pos: PointGeo, region: Region, node: SearchNode): number {
  if (node.depth % 2 === 0) {
    return Math.min(
      dist2lng(pos, region.east, region.south, region.north),
      dist2lng(pos, region.west, region.south, region.north)
    )
  } else {
    return Math.min(
      dist2lat(pos, region.north, region.west, region.east),
      dist2lat(pos, region.south, region.west, region.east)
    )
  }
}

/**
 * 経線上の線分との距離  
 * @param pos この点からの距離を計算
 * @param longitude 経線の経度
 * @param south, north 線分の端点の緯度 (south < north)
 */
function dist2lng(pos: PointGeo, longitude: number, south: number, north: number): number {
  // まず端点との距離
  const dist = [
    measure(pos, { lng: longitude, lat: south }),
    measure(pos, { lng: longitude, lat: north })
  ]
  const d_lng = absLng(pos.lng, longitude)
  if (d_lng <= 90) {
    // 経線への垂線の長さ
    const lng = Math.PI * d_lng / 180
    const lat = Math.PI * pos.lat / 180
    const h = SPHERE_RADIUS * Math.asin(Math.sin(lng) * Math.cos(lat))
    // 垂線の足の緯度
    const y = 90 - 180 * Math.atan2(Math.cos(lng), Math.tan(lat)) / Math.PI
    if (south < y && y < north) {
      dist.push(h)
    }
  }
  return Math.min(...dist)
}

/**
 * 緯線上の線分との距離  
 * @param pos この点からの距離を計算
 * @param latitude 緯線の緯度
 * @param west, east 線分の端点の経度 (west < east)
 */
function dist2lat(pos: PointGeo, latitude: number, west: number, east: number): number {
  // まず端点との距離
  const dist = [
    measure(pos, { lng: east, lat: latitude }),
    measure(pos, { lng: west, lat: latitude })
  ]
  if (west < pos.lng && pos.lng < east) {
    // 垂線の長さ(=経線の一部)
    const h = SPHERE_RADIUS * Math.PI * Math.abs(pos.lat - latitude) / 180
    dist.push(h)
  }
  return Math.min(...dist)
}

/**
 * 経度の差の絶対値  
 * ±360n[deg]の違いは無視する
 * @param lng1 
 * @param lng2 
 * @returns 0 <= diff[deg] <= 180
 */
function absLng(lng1: number, lng2: number): number {
  let lng = lng1 - lng2
  while (lng > 180) lng -= 360
  while (lng < -180) lng += 360
  return Math.abs(lng)
}

const SPHERE_RADIUS = 6371009.0

function measure(a: PointGeo, b: PointGeo) {
  // 大円距離の計算
  const lng1 = Math.PI * a.lng / 180
  const lat1 = Math.PI * a.lat / 180
  const lng2 = Math.PI * b.lng / 180
  const lat2 = Math.PI * b.lat / 180
  const lng = (lng1 - lng2) / 2
  const lat = (lat1 - lat2) / 2
  return SPHERE_RADIUS * 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(lat), 2) + Math.cos(lat1) * Math.cos(lat2) * Math.pow(Math.sin(lng), 2)))
}
```

あとは平面の場合と同様に近傍探索が実現できます。ただし、経度方向は±180[deg]で繋がっているため、探索方向を決定するとき単純な大小比較では不適当な場合があります。

関数`nextWhichChild`の実装を大小比較だけで済ましても探索結果は正しく得られますが、探索するノード数が増えます。特に±180[deg]付近の点を探索する場合（`state.query.lng = ±180`）、  

1. 探索木の浅い段階で不適当な探索方向を選択してしまう
2. 選択しなかった方の探索も必要になる
3. まだまだ深い木を再度探索する

と二分探索木の特性を生かせないおそれがあります。

```ts:search.ts
interface GeodesicSearch {
  node?: SearchNode
  query: PointGeo
  k: number
  result: MeasuredPoint[]
  region: Region // 探索中のnodeの範囲
}

function searchGeodesic(state: GeodesicSearch) {
  const node = state.node
  if (!node) return
  const pos = state.query
  const d = measure(pos, node)
  // ~中略~ state.result の更新

  // まず初めにどちらを探索するか選択
  const which = nextWhichChild(pos, state.region, node)
  searchGeodesic({
    ...state,
    node: which === "left" ? node.left : node.right,
    region: nextRegion(node, state.region, which)
  })

  // もう一方も必要なら探索を実行
  const opposite = (which === "left") ? "right" : "left"
  const region = nextRegion(node, state.region, opposite)
  // 探索しなかった方に存在するかもしれない点との距離の最小値
  const dist2th = minDist2Region(pos, region, node)
  if (dist2th <= state.result[state.result.length - 1].dist) {
    searchGeodesic({
      ...state,
      node: opposite === "left" ? node.left : node.right,
      region: region
    })
  }
}

// どっちにより近い点がありそうか決める
function nextWhichChild(pos: PointGeo, region: Region, node: SearchNode): "left" | "right" {
  if (node.depth % 2 === 0) {
    // 経度方向は±180[deg]で繋がっている
    if (region.west <= pos.lng && pos.lng <= region.east) {
      return pos.lng < node.lng ? "left" : "right"
    } else {
      // 経度の大小では判断できない
      return absLng(pos.lng, region.west) < absLng(pos.lng, region.east) ? "left" : "right"
    }
  } else {
    return pos.lat < node.lat ? "left" : "right"
  }
}

// 次の探索範囲を決定
function nextRegion(node: SearchNode, current: Region, which: "left" | "right"): Region 
```