【Unity + ARFoundation + Lightship】ロケーションベースARを作ろう!(国土地理院方式の距離計算)

# はじめに
こんにちは。私はとあるロケーションベースAR(Location Based AR)アプリを作っているものです。
この記事ではARFoundationを用いてロケーションベースARを作成する方法をご紹介します。
私自身いろいろな記事を調べて作成したので、他の方の書いた記事の受け売りみたいになりそうですが、一つの例として記事を残させていただきます。

# 環境
- MacBook Pro(2024/macOS Sequoia 15.3)
- Unity(2022.3.53f1)
    - ARFoundation(5.1.5)
    - Niantic Lightship AR Plugin(1.4.2)
- iPhone 15(iOS 18.1.1)

# 実装する機能
- 指定した座標へのオブジェクトのスポーン

# プロジェクトのセットアップ
今回はちょっとリッチなオクルージョン(3Dオブジェクトと現実世界の物の前後関係を把握し、遮蔽を表現すること)を導入します。以下の記事を参考にし、プロジェクトをセットアップしてください。

https://qiita.com/HikariNamiki/items/35f4e6b3ae6b16363647

「Sceneの準備」のカメラに`Lightship Occlusion Extension`をアタッチするところまで進めてください。

# 実装
## 指定座標へのオブジェクト生成
実装に入りますが、仕組みとしては現実の2点の座標間(現在位置とオブジェクトを生成したい位置)の距離と方角からUnityのワールド内の座標へと変換してオブジェクトを生成するといった感じです。もう少し詳しい方針は参考にした記事を参照してください。[^1]


:::note info
UnityのARはアプリケーションを起動したところを原点として、スマホを持って移動するとその分`Main Camera`の座標が移動するといったものになっています。
:::

そのため実装する機能としては
- 2点の座標間の直線距離と方角の計算
- 上2つを使ってUnity世界での座標を計算
- オブジェクト生成

の3つです。
座標間の距離計算は[国土地理院が用いている方法](https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/algorithm/bl2st/bl2st.htm)を使います。メートルで距離が出れば、Unity世界での単位`1unit = 1m`なのでUnity世界での座標も出せます。
Unity世界の+Z方向は常に北にはなっていないので端末の方位角と２点の座標間の方位角からUnity世界での角度を出します。あとは三角関数を使ってx軸,z軸の値を出すだけです。

![ForQiita(Bearing).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3949821/19ba2d1d-cf49-4106-b7dc-c741b099ec7f.png)


Unityの`Project`タブから`Scripts`フォルダーを新規作成して、その中で右クリックし`Create`>`C# Script`を選択し、C#ファイルを作成してください。名前はここでは`ObjectSpawner`としておきます。

### 仕様
- 座標情報を保持する構造体(Coordinates)
- 生成したオブジェクトはListに保持する
- スポーン座標計算関数
- 生成したオブジェクトを削除する関数

:::note info
`System.Device.Location`のインポートでエラーが出る場合は、dllの手動での追加が必要です。
- https://www.nuget.org/packages/System.Device.Location.Portable からpackageを入手
- Package内のdllファイルを`Assets`フォルダー内の`Plugin`内に設置する(Pluginフォルダーがない場合は作成する)
:::


```c#: ObjectSpawner.cs
using UnityEngine;
using System;
using System.Collections.Generic;
using System.Device.Location;

[Serializable]
public class SpawnObjectData{
    public double latitude;
    public double longitude;
    public double altitude;
}

[Serializable]
public struct Coordinates
{
    public double latitude;
    public double longitude;
    public double altitude;

    public Coordinates(double latitude, double longitude, double altitude){
        this.latitude = latitude;
        this.longitude = longitude;
        this.altitude = altitude;
    }
}

public class ObjectSpawner : MonoBehaviour
{
    // For coordinates calculate constants
    const double EPSILON = 1e-8;
    const double SEMEMAJOR_AXIS_GRS80 = 6378137;
    const double FLATTENING_GRS80 = 298.257222101;

    // 生成するオブジェクト
    [SerializeField]
    private GameObject spawnObject;

    // 生成したオブジェクトを保持するリスト
    private List<GameObject> spawnedObjects = new List<GameObject>();
    private double target_latitude;
    private double target_longitude;
    private double target_altitude;
    private double current_altitude;

    
    // Start is called before the first frame update
    void Start()
    {
        // compass&Locationシステムの有効化
        Input.compass.enabled = true;
        Input.location.Start();

    }

    // オブジェクト生成関数
    public void PlaceObject(double target_latitude, double target_logitude, double altitude){

        if (Input.location.isEnabledByUser)
        {
            // 現在位置(緯度・経度)を取得
            LocationInfo lastData = Input.location.lastData;
            // 現在の高度を取得
            current_altitude = Input.location.lastData.altitude;

            // Coordinates型に変換
            Coordinates current_coordinates = new Coordinates(lastData.latitude, lastData.longitude, lastData.altitude);
            Coordinates target_coordinates = new Coordinates(target_latitude, target_longitude, target_altitude);

            // 生成場所を計算(返り値はUnity世界のベクトル(x,y,z))
            Vector3 spawnPoint = CalculateSpawnPoint(current_coordinates, target_coordinates);
            // オブジェクト生成(返り値で生成したGameObjectが得られる)
            GameObject spawnedObject = Instantiate(OLDspawnObject, spawnPoint, Quaternion.identity);
            //リストに追加
            spawnedObjects.Add(spawnedObject);
            Debug.Log("|Unity|CurrentCoordinates: " + current_coordinates.latitude + ", " + current_coordinates.longitude);
            Debug.Log("|Unity|TargetCoordinates: " + target_coordinates.latitude + ", " + target_coordinates.longitude);
            Debug.Log("|Unity|--------------------");
            Debug.Log("|Unity|New Calculator Spawn at: " + spawnPoint);
            Debug.Log("|Unity|Spawned Objects: " + String.Join(",", spawnedObjects));


        }


        }

    }

    private bool _eq(double a, double b) {
        return Math.Abs(a - b) < EPSILON;
    }

    // スポーン位置計算
    private Vector3 CalculateSpawnPoint(Coordinates current_coordinates, Coordinates target_coordinates){

        // https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/algorithm/bl2st/bl2st.htm
        // https://www.tandfonline.com/doi/abs/10.1179/sre.1996.33.261.461
        // https://zenn.dev/yonda/articles/c0003d90e52b3e

        //  距離計算
        double current_lat = ToRadian(current_coordinates.latitude);
        double current_lon = ToRadian(current_coordinates.longitude);
        double target_lat = ToRadian(target_coordinates.latitude);
        double target_lon = ToRadian(target_coordinates.longitude);
        const double a = SEMEMAJOR_AXIS_GRS80;
        const double f = 1 / FLATTENING_GRS80;
        double lonDiff = target_lon - current_lon;
        double lonDiffWarp = lonDiff;
        double Delta = lonDiffWarp >= 0 ? target_lat - current_lat : current_lat - target_lat;
        if(lonDiff > Math.PI){
            lonDiffWarp -= 2 * Math.PI;
        }else if(lonDiff < -Math.PI){
            lonDiffWarp += 2 * Math.PI;
        }

        double L = Math.Abs(lonDiffWarp);
        double L_d = Math.PI - L;
        double Sigma = current_lat + target_lat;
        double u1 = lonDiffWarp >= 0 ? Math.Atan((1 - f) * Math.Tan(current_lat)) : Math.Atan((1 - f) * Math.Tan(target_lat));
        double u2 = lonDiffWarp >= 0 ? Math.Atan((1 - f) * Math.Tan(target_lat)) : Math.Atan((1 - f) * Math.Tan(current_lat));
        double Sigma_d = u1 + u2;
        double Delta_d = u2 - u1;
        double xi = Math.Cos(Sigma_d / 2);
        double xi_d = Math.Sin(Sigma_d / 2);
        double eta = Math.Sin(Delta_d / 2);
        double eta_d = Math.Cos(Delta_d / 2);

        double x = Math.Sin(u1) * Math.Sin(u2);
        double y = Math.Cos(u1) * Math.Cos(u2);
        double c = y * Math.Cos(L) + x;
        double epsilon = (f * (2 - f)) / Math.Pow((1 - f), 2);

        double theta = 0;
        int zone = 0;
        int count = 0;
        double Gamma;
        double gamma0;
        double n0 = 0;
        double A = 0;
        double s = 0;
        bool flag = false;
        
        // θ計算
        if(c >= 0){
            theta = L * (1 + f * y);
            zone = 1;
        }else if(c >= -Math.Cos(ToRadian(3) * Math.Cos(u1))){
            theta = L_d;
            zone = 2;
        }else if(c < -Math.Cos(ToRadian(3) * Math.Cos(u1))){
            zone = 3;
            double R = f * Math.PI * Math.Pow(Math.Cos(u1), 2) * (1 - (f * (1 + f) * Math.Pow(Math.Sin(u1), 2)) / 4 + (3.0 / 16.0) * Math.Pow(f, 2) * Math.Pow(Math.Sin(u1), 4));
            double d1 = L_d * Math.Cos(u1) - R;
            double d2 = Math.Abs(Sigma_d) + R;
            double q = L_d / (f * Math.PI);
            double f1 = (f * (1 + f / 2.0)) / 4.0;
            gamma0 = q + f1 * q - f1 * Math.Pow(q, 3);

            // Sigma == 0
            if(_eq(Sigma, 0)){
                if(d1 > 0){
                    // b1
                    theta = L_d;
                    
                }else if(_eq(d1, 0)){//d1 == 0
                    // b2
                    Gamma = Math.Pow(Math.Sin(u1), 2);
                    n0 = (epsilon * Gamma) / Math.Pow(Math.Sqrt(1 + epsilon * Gamma) + 1, 2);
                    A = (1 + n0) * (1 + (5 / 4) * Math.Pow(n0, 2));
                    s = (1 - f) * a * A * Math.PI;
                    flag = true;
                }else {
                    // b3
                    count = 0;
                    Gamma = 0;
                    gamma0 = 0;
                    while(count < 1000){
                        Gamma = 1 - Math.Pow(gamma0, 2);
                        double D = f * (1 + f) / 4 - 3 / 16 * Math.Pow(f, 2) * Gamma;
                        double gamma1 = q * (1 - 1 * Gamma);
                        if(Math.Abs(gamma0 - gamma1) < Math.Pow(10, -14)) break;
                        gamma0 = gamma1;
                        count += 1;
                    }
                    if(count >= 1001){
                        Debug.Log("|Unity|ERROR");
                        throw new Exception();
                    }
                    n0 = (epsilon * Gamma) / Math.Pow((Math.Sqrt(1 + epsilon * Gamma) + 1), 2);
                    A = (1 + n0) * (1 + (5 /4) * Math.Pow(n0, 2));
                    s = (1 - f) * a * A * Math.PI;
                    flag = true;
                }
            }else {
                // a1
                double A0 = Math.Atan(d1 / d2);
                double B0 = Math.Asin(R / Math.Sqrt(Math.Pow(d1, 2) + Math.Pow(d2, 2)));
                double psi = A0 + B0;
                double j = gamma0 / Math.Cos(u1);
                double k = ((1 + f1) * Math.Abs(Sigma_d) * (1 - f * y)) / (f * Math.PI * y);
                double j1 = j / (1 + k * Math.Sin(psi));
                double psi_d = Math.Asin(j1);
                double psi_dd = Math.Asin((j1 * Math.Cos(u1)) / Math.Cos(u2));
                theta = 2 * Math.Atan(
                    (Math.Tan((psi_d + psi_dd) / 2) * Math.Sin(Math.Abs(Sigma_d) / 2)) 
                    / Math.Cos(Delta_d / 2)
                );


            }
        }

        if(!flag){
            count = 0;
            Gamma = 0;
            double sigma = 0, zeta = 0, J = 0, K = 0;
            while(count < 1000){

                double g = zone == 1
                    ? Math.Sqrt(Math.Pow(eta, 2) * Math.Pow(Math.Cos(theta / 2), 2) + Math.Pow(xi, 2) * Math.Pow(Math.Sin(theta / 2), 2))
                    : Math.Sqrt(Math.Pow(eta, 2) * Math.Pow(Math.Sin(theta / 2), 2) + Math.Pow(xi, 2) * Math.Pow(Math.Cos(theta / 2), 2));

                double h = zone == 1
                    ? Math.Sqrt(Math.Pow(eta_d, 2) * Math.Pow(Math.Cos(theta / 2), 2) + Math.Pow(xi_d, 2) * Math.Pow(Math.Sin(theta / 2), 2))
                    : Math.Sqrt(Math.Pow(eta_d, 2) * Math.Pow(Math.Sin(theta / 2), 2) + Math.Pow(xi_d, 2) * Math.Pow(Math.Cos(theta / 2), 2));
                sigma = 2 * Math.Atan(g / h);
                J = 2 * g * h;
                K = Math.Pow(h, 2) - Math.Pow(g, 2);
                double gamma = (y * Math.Sin(theta)) / J;
                Gamma = 1 - Math.Pow(gamma, 2);
                zeta = Gamma * K - 2 * x;
                double zeta_d = zeta + x;
                double D = (f * (1 + f)) / 4 - (3.0 / 16.0) * Math.Pow(f, 2) * Gamma;
                double E = (1 - D * Gamma) * f * gamma * (sigma + D * J * (zeta + D * K * (2 * Math.Pow(zeta, 2) - Math.Pow(Gamma, 2))));
                double F = zone == 1 ? theta - L - E : theta - L_d + E;
                double G = f * Math.Pow(gamma, 2) * (1 - 2 * D * Gamma) + 
                ((f * zeta_d * sigma) / J) * (1 - D * Gamma + (f * Math.Pow(gamma, 2)) / 2) + 
                (Math.Pow(f, 2) * zeta * zeta_d) / 4;
                theta -= F / (1 - G);

                if(Math.Abs(F) < 1e-14) break;
                
                count++;
            }
            if(count >= 1001){
                Debug.Log("|Unity|ERROR");
                throw new Exception();
            }

            n0 = (epsilon * Gamma) / Math.Pow((Math.Sqrt(1 + epsilon * Gamma) + 1), 2);
            A = (1 + n0) * (1 + (5.0 / 4.0) * Math.Pow(n0, 2));
            double B = (epsilon * (1 - (3 * Math.Pow(n0, 2)) / 8)) / (Math.Pow(Math.Sqrt(1 + epsilon * Gamma) + 1, 2));
            s = (1 - f) * a * A * (sigma - B * J * (zeta - (B * (K * (Math.Pow(Gamma, 2) - 2 * Math.Pow(zeta, 2)) - (B * zeta * (1 - 4 * Math.Pow(K, 2)) * (3 * Math.Pow(Gamma, 2) - 4 * Math.Pow(zeta, 2))) / 6)) / 4));
        }
        double alfa, alfa_half, alfa_d, alfa1_d, alfa2, alfa21_d, alfa1, alfa21;
        if(zone == 1){
                alfa = Math.Atan((xi / eta) * Math.Tan(theta / 2));
                alfa_half = Math.Atan((xi_d / eta_d) * Math.Tan(theta / 2));
                alfa_d = ((alfa >= 0 && L >= 0) || (alfa < 0 && L == 0)) ? alfa : alfa + ToRadian(180);
                alfa1_d = alfa_d - alfa_half;
                alfa2 = alfa_d + alfa_half;
                
        }else{
            alfa = Math.Atan((xi_d / eta_d) * Math.Tan(theta / 2));
            alfa_half = Math.Atan((xi / eta) * Math.Tan(theta / 2));
            alfa_d = ((alfa >= 0 && L >= 0) || (alfa < 0 && L == 0)) ? alfa : alfa + ToRadian(180);
            alfa1_d = alfa_d - alfa_half;
            alfa2 = ToRadian(180) - alfa_d - alfa_half;
        }
        alfa21_d = ToRadian(180) + alfa2;
        alfa1 = lonDiffWarp >= ToRadian(0) ? alfa1_d : alfa21_d;
        alfa21 = lonDiffWarp >= ToRadian(0) ? alfa21_d : alfa1_d;
        if(alfa1 < ToRadian(0)){
            alfa1 += ToRadian(360) * (int)(Math.Abs(alfa1) / ToRadian(360));
        }else if(ToRadian(360) < alfa1){
            alfa1 -= ToRadian(360) * (int)(Math.Abs(alfa1) / ToRadian(360));
        }
        if(alfa21 < ToRadian(0)){
            alfa21 += ToRadian(360) * (int)(Math.Abs(alfa21) / ToRadian(360));
        }else if(ToRadian(360) < alfa21){
            alfa21 -= ToRadian(360) * (int)(Math.Abs(alfa21) / ToRadian(360));
        }

        if((L == ToRadian(0) && Delta >= 0) || (Math.Abs(L) == ToRadian(180) && Sigma >= 0)){
            alfa1 = ToRadian(0);
        }else if((L == ToRadian(0) && Delta < 0) || (Math.Abs(L) == ToRadian(180) && Sigma < 0)){
            alfa1 = ToRadian(180);
        }
        if((L == ToRadian(0) && Delta < 0) || (Math.Abs(L) == ToRadian(180) && Sigma >= 0))
        {
            alfa21 = ToRadian(0);
        }else if((L == ToRadian(0) && Delta >= 0) || (Math.Abs(L) == ToRadian(180) && Sigma < 0)){
            alfa21 = ToRadian(180);
        }

        // 距離: s, 現在地から目的地の方位角: alfa1, 目的地から現在地の方位角: alfa21(使わない)
        
        // 端末が向いている方角を取得
        float phone_deg = Input.compass.trueHeading;

        Debug.Log("|Unity|Calculate Result-------Distance: "+ s + ", BearingFromCurrent: "+ ToDegree(alfa1) + ", BearingFromTarget: "+ ToDegree(alfa21) + "Revised: " + (ToDegree(alfa1) - phone_deg));

        double distance = s;
        double bearing = alfa1 - ToRadian(phone_deg);

        return new Vector3(
            (float)(Math.Sin(bearing) * distance),
            (float)(target_coordinates.altitude - current_altitude),
            (float)(Math.Cos(bearing) * distance)
        );
    }



    private double ToRadian(double degree)
    {
        return degree * Math.PI / 180;
    }

    private double ToDegree(double radian)
    {
        return radian * 180 / Math.PI;
    }

    public void DeleteSpawnedObjects()
    {
        if(spawnedObjects.Count != 0){
            foreach(GameObject i in spawnedObjects){
                Destroy(i);
            }
        }
        
        spawnedObjects.Clear();
    }



}
```

このコードを適当なGameObjectにアタッチして、インスペクターから`spawnObject`に生成したいPrefabをアタッチして、何かしらのスクリプトから`ObjectSpawner.PlaceObject([緯度], [経度], [高度]);`を叩けば、現実と同じような距離感でオブジェクトが生成されます。
`ObjectSpawner.DeleteSpawnedObjects()`でスポーンしたオブジェクトが消えます。

ここから先はご自身の好きなように組み込んでください。

# おわりに
国土地理院方式でC#の計算方法を載せている記事がなかったので書いてみました。他の計算方法もいいですけど、やっぱり国土地理院のネームバリューには惹かれてしまいますね。
次回、コンパスバーについて記事を書こうと思います。生成したオブジェクトがどの方向にあるのか方角と一緒に確認できるようにしましょう。
それでは、また。

# 参考

https://qiita.com/ta-sr/items/cff5d64d973c84dd08b8

https://qiita.com/Yuzu2yan/items/0f312954feeb3c83c70e

https://zenn.dev/yonda/articles/c0003d90e52b3e

https://qiita.com/Ryopon/items/dc659a3170e092a9653d

https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2stf.html

[^1]: https://qiita.com/ta-sr/items/cff5d64d973c84dd08b8