緯度経度と直交座標を相互変換する

はじめに

緯度経度高さで表される座標と、XYZの三次元直交座標を相互変換します。
Unityで使いたかったため、C#とUnityのタグをつけていますが、特にUnityに制限されるものではありません。

計算方法については、「世界測地系と座標変換」（飛田幹夫著、日本測量協会発行）を参考にしました。
世界測地系と座標変換（Amazon）
この本は日本測量協会に問い合わせれば定価で購入できると思います。

※注意※
著者は本稿の内容について、精度や正確さ正しさを必ずしも保証するものではありません。
著者は本稿の内容を利用したことにより起こった事柄に関して一切の責任を負わないものとします。
実際に使われる場合は、値域の制限などを入れることや、精度保証などのテストを行って、ご自身の責任においてご利用ください。

前提

緯度経度はWGS84に基づいているものとします。変換パラメータを変えれば、他の測地系にも対応できると思いますが、本稿の範囲外とします。
緯度は北側が+、南側に-で、-90~90度。経度は東に+、西に-で、-180~180度とします。
高さは、WGS84に基づく楕円体高でメートルを単位とします。標高ではないのでご注意ください。
XYZ座標は、+X軸が子午線、東向きに+Y軸、北向きに+Z軸とします。こちらも単位はメートルになります。

プログラム

CoordConv.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

using System;

/// <summary>
/// 座標の変換用クラス。なるべくシンプルに必要な変換だけ。
/// 測地系はWGS84を使用。
/// 計算方法は「世界測地系と座標変換　飛田幹夫　日本測量協会」を参考にした。
/// </summary>
public class CoordConv
{
    private const double a = 6378137.0;
    private const double f = 1.0 / 298.257223563;
    private const double e2 = f * (2.0 - f);

    /// <summary>
    /// 緯度経度（WGS84）から直交座標（ECEF）に変換するメソッド。
    /// </summary>
    /// <param name="b">緯度</param>
    /// <param name="l">経度</param>
    /// <param name="h">楕円体高</param>
    /// <returns>直交座標のXYZ。+Zが北極、+Xが子午線、+Yが東経方向</returns>
    public static (double x, double y, double z) BLH2XYZ(double b, double l, double h){
        b = Math.PI * b / 180.0;
        l = Math.PI * l / 180.0;

        double N = a / Math.Sqrt(1.0 - e2 * Math.Pow(Math.Sin(b),2.0));

        return (
            (N + h) * Math.Cos(b) * Math.Cos(l),
            (N + h) * Math.Cos(b) * Math.Sin(l),
            (N * (1.0 - e2) + h) * Math.Sin(b) 
        );
    }

    /// <summary>
    /// 直交座標（ECEF）から緯度経度（WGS84）に変換するメソッド。
    /// </summary>
    /// <param name="x">X</param>
    /// <param name="y">Y</param>
    /// <param name="z">Z</param>
    /// <returns>緯度経度。b緯度、l経度、h楕円体高</returns>
    public static (double b, double l, double h) XYZ2BLH(double x, double y, double z){
        double p = Math.Sqrt(x*x + y*y);
        double r = Math.Sqrt(p*p + z*z);
        double mu = Math.Atan(z / p * ((1.0 - f) + e2 * a/r));

        double B = Math.Atan( (z * (1.0-f) + e2*a*Math.Pow(Math.Sin(mu),3)) / ((1.0-f)*(p-e2*a*Math.Pow(Math.Cos(mu),3))) );
        return (
            180.0 * B / Math.PI,
            180.0 * Math.Atan2(y,x) / Math.PI,
            p * Math.Cos(B) + z*Math.Sin(B) - a*Math.Sqrt(1.0 - e2*Math.Pow(Math.Sin(B),2))
        );
    }
}

注意点

必ず、doubleで値を渡してください。floatでは一般的な用途でも演算の精度が不足する可能性が高いです。得られた値は、なるべく小さな値になるようにdoubleのまま平行移動などしてからfloatに変換し、Unityのオブジェクトの座標（Vector3など）に代入してください。
計算部分では値域のチェックなどもしていません。必要なら追加してください。
変換パラメータがWGS84の改訂などで変わることがあります。高い精度を求める場合は都度信頼できる情報を確認してください。