From e8e0ca0b7c87feb38c53a6fd0b163e083b9d4477 Mon Sep 17 00:00:00 2001
From: Oscar Lorentzon <oscarlorentzon@users.noreply.github.com>
Date: Fri, 8 Mar 2024 16:28:18 +0100
Subject: [PATCH] feat: use bearings for fov visualization

---
 src/component/bearing/BearingComponent.ts | 37 +++++----------------
 src/geo/Geo.ts                            | 40 +++++++++++++++++++++++
 src/geo/Spatial.ts                        | 18 ++++++++++
 3 files changed, 67 insertions(+), 28 deletions(-)

diff --git a/src/component/bearing/BearingComponent.ts b/src/component/bearing/BearingComponent.ts
index f4774baf2..115041a36 100644
--- a/src/component/bearing/BearingComponent.ts
+++ b/src/component/bearing/BearingComponent.ts
@@ -130,12 +130,7 @@ export class BearingComponent extends Component<BearingConfiguration> {
                             return [Math.PI, Math.PI];
                         }
 
-                        const currentProjectedPoints =
-                            this._computeProjectedPoints(transform);
-                        const hFov = this._spatial
-                            .degToRad(
-                                this._computeHorizontalFov(
-                                    currentProjectedPoints));
+                        const hFov = this._computeHorizontalFov(transform);
 
                         let hFovLeft: number = hFov / 2;
                         let hFovRight: number = hFov / 2;
@@ -469,37 +464,23 @@ export class BearingComponent extends Component<BearingConfiguration> {
             ]);
     }
 
-    private _computeProjectedPoints(transform: Transform): number[][] {
+    private _computeHorizontalFov(transform: Transform): number {
         const vertices: number[][] = [[1, 0]];
         const directions: number[][] = [[0, 0.5]];
         const pointsPerLine: number = 12;
 
-        return Geo
-            .computeProjectedPoints(
-                transform,
-                vertices,
-                directions,
-                pointsPerLine,
-                this._viewportCoords)
-            .map(([x, y]) => [Math.abs(x), Math.abs(y)]);
-    }
-
-    private _computeHorizontalFov(projectedPoints: number[][]): number {
-        const fovs: number[] = projectedPoints
-            .map(
-                (projectedPoint: number[]): number => {
-                    return this._coordToFov(projectedPoint[0]);
-                });
+        const bearings = Geo.computeBearings(
+            transform, vertices, directions, pointsPerLine, this._viewportCoords);
+        const projections = bearings
+            .map(b => this._spatial.projectToPlane(b, [0, 1, 0]))
+            .map(p => [p[0], p[2]]);
 
-        const fov: number = Math.min(...fovs);
+        const angles = projections.map(p => Math.atan2(p[0], -p[1]));
+        const fov = 2 * Math.min(...angles);
 
         return fov;
     }
 
-    private _coordToFov(x: number): number {
-        return this._spatial.radToDeg(2 * Math.atan(x));
-    }
-
     private _interpolate(x1: number, x2: number, alpha: number): number {
         return (1 - alpha) * x1 + alpha * x2;
     }
diff --git a/src/geo/Geo.ts b/src/geo/Geo.ts
index 7be455f98..1ae63036e 100644
--- a/src/geo/Geo.ts
+++ b/src/geo/Geo.ts
@@ -31,6 +31,46 @@ export function computeTranslation(position: LngLatAlt, rotation: number[], refe
     return translation;
 }
 
+export function computeBearings(
+    transform: Transform,
+    basicVertices: number[][],
+    basicDirections: number[][],
+    pointsPerLine: number,
+    viewportCoords: ViewportCoords): number[][] {
+
+    // @ts-ignore
+    const camera: THREE.Camera = new THREE.Camera();
+    camera.up.copy(transform.upVector());
+    camera.position.copy(new THREE.Vector3().fromArray(transform.unprojectSfM([0, 0], 0)));
+    camera.lookAt(new THREE.Vector3().fromArray(transform.unprojectSfM([0, 0], 10)));
+    camera.updateMatrix();
+    camera.updateMatrixWorld(true);
+
+    const basicPoints: number[][] = [];
+    for (let side: number = 0; side < basicVertices.length; ++side) {
+        const v: number[] = basicVertices[side];
+        const d: number[] = basicDirections[side];
+
+        for (let i: number = 0; i <= pointsPerLine; ++i) {
+            basicPoints.push([
+                v[0] + d[0] * i / pointsPerLine,
+                v[1] + d[1] * i / pointsPerLine,
+            ]);
+        }
+    }
+
+    const bearings: number[][] = [];
+    for (const [index, basicPoint] of basicPoints.entries()) {
+        const worldPoint = transform.unprojectBasic(basicPoint, 10000);
+        const cameraPoint = new THREE.Vector3()
+            .fromArray(viewportCoords.worldToCamera(worldPoint, camera));
+        cameraPoint.normalize();
+        bearings.push(cameraPoint.toArray());
+    }
+
+    return bearings;
+}
+
 export function computeProjectedPoints(
     transform: Transform,
     basicVertices: number[][],
diff --git a/src/geo/Spatial.ts b/src/geo/Spatial.ts
index 313d1baa9..3b48ca680 100644
--- a/src/geo/Spatial.ts
+++ b/src/geo/Spatial.ts
@@ -230,6 +230,24 @@ export class Spatial {
         return Math.asin(projection / norm);
     }
 
+    /**
+     * Calculates the projection of a vector onto a plane.
+     *
+     * @param {Array<number>} vector - The vector.
+     * @param {Array<number>} planeNormal - Normal of the plane.
+     * @returns {number} Projection of vector onto plane.
+     */
+    public projectToPlane(vector: number[], planeNormal: number[]): number[] {
+        const directionVector: THREE.Vector3 = new THREE.Vector3().fromArray(vector);
+        const normalVector: THREE.Vector3 = new THREE.Vector3().fromArray(planeNormal);
+
+        const normalProjection: number = directionVector.clone().dot(normalVector);
+        const planeProjection: THREE.Vector3 = directionVector
+            .clone().sub(normalVector.clone().multiplyScalar(normalProjection));
+
+        return planeProjection.toArray();
+    }
+
     public azimuthal(direction: number[], up: number[]): number {
         const directionVector: THREE.Vector3 = new THREE.Vector3().fromArray(direction);
         const upVector: THREE.Vector3 = new THREE.Vector3().fromArray(up);