Only use rotation matrix for AR

kylecorry31 · Nov 5, 2023 · 015dd73 · 015dd73
1 parent 170415c
commit 015dd73
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 43 deletions.
diff --git a/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt b/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt
@@ -101,26 +101,6 @@ object AugmentedRealityUtils {
         return getPixelLinear(spherical.first, 0f, spherical.second, 0f, size, fov)
     }
 
-    /**
-     * Gets the pixel coordinate of a point on the screen given the bearing and azimuth.
-     * @param bearing The compass bearing in degrees of the point
-     * @param elevation The elevation in degrees of the point
-     * @param quaternion The quaternion of the device in the AR coordinate system
-     * @param size The size of the view in pixels
-     * @param fov The field of view of the camera in degrees
-     */
-    fun getPixel(
-        bearing: Float,
-        elevation: Float,
-        quaternion: Quaternion,
-        size: Size,
-        fov: Size
-    ): PixelCoordinate {
-        val spherical = toRelative(bearing, elevation, 1f, quaternion)
-        // The rotation of the device has been negated, so azimuth = 0 and inclination = 0 is used
-        return getPixelLinear(spherical.first, 0f, spherical.second, 0f, size, fov)
-    }
-
     /**
      * Computes the orientation of the device in the AR coordinate system.
      * @param orientationSensor The orientation sensor

diff --git a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt
@@ -4,6 +4,7 @@ import android.annotation.SuppressLint
 import android.content.Context
 import android.graphics.Color
 import android.graphics.Path
+import android.hardware.SensorManager
 import android.opengl.Matrix
 import android.util.AttributeSet
 import android.view.GestureDetector
@@ -18,6 +19,7 @@ import com.kylecorry.andromeda.sense.clinometer.SideClinometer
 import com.kylecorry.andromeda.sense.orientation.IOrientationSensor
 import com.kylecorry.sol.math.Euler
 import com.kylecorry.sol.math.Quaternion
+import com.kylecorry.sol.math.QuaternionMath
 import com.kylecorry.sol.math.SolMath.real
 import com.kylecorry.sol.math.SolMath.toDegrees
 import com.kylecorry.sol.math.SolMath.toRadians
@@ -59,14 +61,11 @@ class AugmentedRealityView : CanvasView {
 
     var backgroundFillColor: Int = Color.TRANSPARENT
 
-    // TODO: Remove legacy orientation in favor of a custom orientation sensor
-    private var legacyOrientation = Quaternion.zero
     private var rotationMatrix = FloatArray(16)
     private val quaternion = FloatArray(4)
     private val orientation = FloatArray(3)
     private var size = Size(width.toFloat(), height.toFloat())
 
-
     // Sensors / preferences
     private val userPrefs = UserPreferences(context)
     private val sensors = SensorService(context)
@@ -385,23 +384,13 @@ class AugmentedRealityView : CanvasView {
     fun toPixel(coordinate: HorizonCoordinate): PixelCoordinate {
         val bearing = getActualBearing(coordinate)
 
-        return if (orientationSensor == null) {
-            AugmentedRealityUtils.getPixel(
-                bearing,
-                coordinate.elevation,
-                legacyOrientation,
-                size,
-                fov
-            )
-        } else {
-            AugmentedRealityUtils.getPixel(
-                bearing,
-                coordinate.elevation,
-                rotationMatrix,
-                size,
-                fov
-            )
-        }
+        return AugmentedRealityUtils.getPixel(
+            bearing,
+            coordinate.elevation,
+            rotationMatrix,
+            size,
+            fov
+        )
     }
 
     fun toPixel(coordinate: Coordinate, elevation: Float? = null): PixelCoordinate {
@@ -438,9 +427,14 @@ class AugmentedRealityView : CanvasView {
 
         val orientationSensor = orientationSensor
         if (orientationSensor == null) {
-            // TODO: This fails when the device is pointed almost straight up or down
-            legacyOrientation =
-                Quaternion.from(Euler(inclination, -sideInclination, -azimuth)).inverse()
+            QuaternionMath.fromEuler(
+                floatArrayOf(inclination, -sideInclination, -azimuth),
+                quaternion
+            )
+            SensorManager.getRotationMatrixFromVector(
+                rotationMatrix,
+                quaternion
+            )
             return
         }