Add pixel to ar coordinate (linear)

Signed-off-by: Kyle Corry <[email protected]>

app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt

@@ -14,6 +14,7 @@ import com.kylecorry.sol.units.Coordinate
 import com.kylecorry.sol.units.Distance
 import com.kylecorry.trail_sense.tools.augmented_reality.domain.mapper.CameraAnglePixelMapper
 import com.kylecorry.trail_sense.tools.augmented_reality.domain.mapper.LinearCameraAnglePixelMapper
+import com.kylecorry.trail_sense.tools.augmented_reality.domain.position.SphericalARPoint
 import kotlin.math.abs
 import kotlin.math.atan2
 import kotlin.math.cos
@@ -23,6 +24,7 @@ object AugmentedRealityUtils {
 
     private val worldVectorLock = Any()
     private val tempWorldVector = FloatArray(4)
+    private val tempRotationMatrix = FloatArray(16)
 
     // Constants for perspective projection
     private const val minDistance = 0.1f
@@ -31,7 +33,7 @@ object AugmentedRealityUtils {
     private val linear = LinearCameraAnglePixelMapper()
     private val rect = RectF()
     private val rectLock = Any()
-    
+
     /**
      * Gets the pixel coordinate of a point on the screen given the bearing and azimuth. The point is considered to be on a plane.
      * @param bearing The compass bearing in degrees of the point
@@ -128,6 +130,29 @@ object AugmentedRealityUtils {
         )
     }
 
+    fun getCoordinate(
+        pixel: PixelCoordinate,
+        rotationMatrix: FloatArray,
+        rect: RectF,
+        fov: Size,
+        mapper: CameraAnglePixelMapper
+    ): Vector3 {
+        val world = mapper.getAngle(pixel.x, pixel.y, rect, fov)
+        // TODO: Get this working for all mappers
+//        val inversePerspective = Optics.inversePerspectiveProjection(
+////            Vector2(pixel.x, pixel.y),
+//            pixel.toVector2(rect.top),
+//            Vector2(
+//                Optics.getFocalLength(fov.width, rect.width()),
+//                Optics.getFocalLength(fov.height, rect.height())
+//            ),
+//            PixelCoordinate(rect.centerX(), rect.centerY()).toVector2(rect.top),
+//            100f
+//        )
+        val spherical = SphericalARPoint(world.x, world.y).coordinate.position
+        return arToEnu(spherical, rotationMatrix)
+    }
+
 
     /**
      * Computes the orientation of the device in the AR coordinate system.
@@ -208,5 +233,20 @@ object AugmentedRealityUtils {
         }
     }
 
+    fun arToEnu(ar: Vector3, rotationMatrix: FloatArray): Vector3 {
+        return synchronized(worldVectorLock) {
+            tempWorldVector[0] = ar.x
+            tempWorldVector[1] = ar.y
+            tempWorldVector[2] = ar.z
+            tempWorldVector[3] = 1f
+
+            // Invert the rotation matrix
+            Matrix.invertM(tempRotationMatrix, 0, rotationMatrix, 0)
+
+            Matrix.multiplyMV(tempWorldVector, 0, tempRotationMatrix, 0, tempWorldVector, 0)
+            Vector3(tempWorldVector[0], tempWorldVector[1], tempWorldVector[2])
+        }
+    }
+
 }
 

app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityView.kt

@@ -447,30 +447,19 @@ class AugmentedRealityView : CanvasView {
 
     fun toCoordinate(
         pixel: PixelCoordinate,
-        isClippedToScreen: Boolean = false,
-        azimuthOverride: Float? = null,
-        inclinationOverride: Float? = null
+        rotationMatrixOverride: FloatArray? = null
     ): AugmentedRealityCoordinate {
-        // TODO: Adjust for side inclination
-        // TODO: Allow the user to pass in a rotation matrix (rather than azimuth/inclination)
-        val mapper = if (camera?.isStarted == true) cameraMapper ?: defaultMapper else defaultMapper
-        val screenPixel = Vector3(pixel.x + x, pixel.y + y, 0f)
-        var rect = previewRect ?: RectF(0f, 0f, width.toFloat(), height.toFloat())
-        if (isClippedToScreen) {
-            rect = RectF(
-                rect.left.coerceAtLeast(0f),
-                rect.top.coerceAtLeast(0f),
-                rect.right.coerceAtMost(width.toFloat()),
-                rect.bottom.coerceAtMost(height.toFloat())
-            )
-        }
-        val angle = mapper.getAngle(screenPixel.x, screenPixel.y, rect, fov)
-        return AugmentedRealityCoordinate.fromSpherical(
-            (azimuthOverride ?: azimuth) + angle.x,
-            (inclinationOverride ?: inclination) + angle.y,
-            Float.MAX_VALUE,
-            isTrueNorth
+        val screenPixel = PixelCoordinate(pixel.x + x, pixel.y + y)
+        val rect = previewRect ?: RectF(0f, 0f, width.toFloat(), height.toFloat())
+        val coordinate = AugmentedRealityUtils.getCoordinate(
+            screenPixel,
+            rotationMatrixOverride ?: rotationMatrix,
+            rect,
+            fov,
+            if (camera?.isStarted == true) cameraMapper ?: defaultMapper else defaultMapper
         )
+
+        return AugmentedRealityCoordinate(coordinate, isTrueNorth)
     }
 
     fun getActualPoint(point: Vector3, isPointTrueNorth: Boolean): Vector3 {

app/src/main/java/com/kylecorry/trail_sense/tools/celestial_navigation/ui/CelestialNavigationFragment.kt

@@ -228,6 +228,7 @@ class CelestialNavigationFragment : BoundFragment<FragmentCelestialNavigationBin
     }
 
     private fun recordStar() {
+        val rotationMatrix = binding.arView.rotationMatrix.copyOf()
         var inclination = binding.arView.inclination
         // TODO: Maybe set true north to false and calculate using a location suggested by the user
         var azimuth = Bearing.getBearing(binding.arView.azimuth)
@@ -249,9 +250,7 @@ class CelestialNavigationFragment : BoundFragment<FragmentCelestialNavigationBin
                         val markers = starPixels.map {
                             val point = binding.arView.toCoordinate(
                                 it,
-                                isClippedToScreen = true,
-                                azimuthOverride = azimuth,
-                                inclinationOverride = inclination
+                                rotationMatrixOverride = rotationMatrix
                             )
                             ARMarker(
                                 SphericalARPoint(point.bearing, point.elevation),
@@ -268,9 +267,7 @@ class CelestialNavigationFragment : BoundFragment<FragmentCelestialNavigationBin
                     } ?: return@launch
                     val arPoint = binding.arView.toCoordinate(
                         nearestToCenter,
-                        isClippedToScreen = true,
-                        azimuthOverride = azimuth,
-                        inclinationOverride = inclination
+                        rotationMatrixOverride = rotationMatrix
                     )
                     azimuth = arPoint.bearing
                     inclination = arPoint.elevation