Use rotation matrix for AR

app/src/main/java/com/kylecorry/trail_sense/shared/sensors/SensorService.kt

@@ -193,6 +193,10 @@ class SensorService(ctx: Context) {
         return CompassProvider(context, userPrefs.compass).get()
     }
 
+    fun getOrientation(): IOrientationSensor? {
+        return CompassProvider(context, userPrefs.compass).getOrientationSensor()
+    }
+
     fun getDeviceOrientationSensor(): DeviceOrientation {
         // While not technically an environment sensor, it doesn't need to update often - and can match their rate
         return DeviceOrientation(context, ENVIRONMENT_SENSOR_DELAY)

app/src/main/java/com/kylecorry/trail_sense/shared/sensors/compass/NullOrientationSensor.kt

@@ -0,0 +1,12 @@
+package com.kylecorry.trail_sense.shared.sensors.compass
+
+import com.kylecorry.andromeda.sense.orientation.IOrientationSensor
+import com.kylecorry.sol.math.Quaternion
+import com.kylecorry.trail_sense.shared.sensors.NullSensor
+
+class NullOrientationSensor : NullSensor(), IOrientationSensor {
+    override val orientation: Quaternion
+        get() = Quaternion.zero
+    override val rawOrientation: FloatArray
+        get() = FloatArray(4)
+}

app/src/main/java/com/kylecorry/trail_sense/shared/sensors/providers/CompassProvider.kt

@@ -10,13 +10,16 @@ import com.kylecorry.andromeda.sense.compass.ICompass
 import com.kylecorry.andromeda.sense.compass.LegacyCompass
 import com.kylecorry.andromeda.sense.magnetometer.Magnetometer
 import com.kylecorry.andromeda.sense.orientation.GeomagneticRotationSensor
+import com.kylecorry.andromeda.sense.orientation.IOrientationSensor
+import com.kylecorry.andromeda.sense.orientation.OrientationSensor
 import com.kylecorry.andromeda.sense.orientation.RotationSensor
 import com.kylecorry.sol.math.filters.MovingAverageFilter
 import com.kylecorry.trail_sense.settings.infrastructure.ICompassPreferences
 import com.kylecorry.trail_sense.shared.sensors.SensorService
 import com.kylecorry.trail_sense.shared.sensors.compass.CompassSource
 import com.kylecorry.trail_sense.shared.sensors.compass.MagQualityCompassWrapper
 import com.kylecorry.trail_sense.shared.sensors.compass.NullCompass
+import com.kylecorry.trail_sense.shared.sensors.compass.NullOrientationSensor
 
 class CompassProvider(private val context: Context, private val prefs: ICompassPreferences) {
 
@@ -65,6 +68,37 @@ class CompassProvider(private val context: Context, private val prefs: ICompassP
         )
     }
 
+    fun getOrientationSensor(): IOrientationSensor? {
+        // TODO: This isn't used by the actual orientation sensors (they should use it)
+        val useTrueNorth = prefs.useTrueNorth
+
+        var source = prefs.source
+
+        // Handle if the available sources have changed (not likely)
+        val allSources = getAvailableSources(context)
+
+        // There were no compass sensors found
+        if (allSources.isEmpty()){
+            return NullOrientationSensor()
+        }
+
+        if (!allSources.contains(source)) {
+            source = allSources.firstOrNull() ?: CompassSource.CustomMagnetometer
+        }
+
+        // TODO: Apply the smoothing / quality to the orientation sensor
+        if (source == CompassSource.RotationVector){
+            return RotationSensor(context, useTrueNorth, SensorService.MOTION_SENSOR_DELAY)
+        }
+
+        if (source == CompassSource.GeomagneticRotationVector){
+            return GeomagneticRotationSensor(context, useTrueNorth, SensorService.MOTION_SENSOR_DELAY)
+        }
+
+        // TODO: Construct this from existing sensors
+        return null
+    }
+
     companion object {
         /**
          * Returns the available compass sources in order of quality

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

@@ -72,10 +72,6 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
 
     private var isCameraEnabled = true
 
-    private val compassSyncTimer = CoroutineTimer {
-        binding.linearCompass.azimuth = Bearing(binding.arView.azimuth)
-    }
-
     // TODO: Draw an indicator around the focused marker
 
     override fun onViewCreated(view: View, savedInstanceState: Bundle?) {
@@ -99,9 +95,6 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
         binding.camera.setScaleType(PreviewView.ScaleType.FIT_CENTER)
         binding.camera.setShowTorch(false)
 
-        // TODO: Show azimuth / altitude
-        binding.linearCompass.showAzimuthArrow = false
-
         binding.arView.setLayers(listOf(northLayer, horizonLayer, sunLayer, moonLayer, beaconLayer))
 
         binding.cameraToggle.setOnClickListener {
@@ -123,8 +116,6 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
             startCamera()
         }
         updateAstronomyLayers()
-
-        compassSyncTimer.interval(INTERVAL_60_FPS)
     }
 
     // TODO: Move this to the AR view
@@ -157,7 +148,6 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
         super.onPause()
         binding.camera.stop()
         binding.arView.stop()
-        compassSyncTimer.stop()
     }
 
     override fun onUpdate() {
@@ -166,7 +156,6 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
         // TODO: Move this to a coroutine (and to the AR view)
         val fov = binding.camera.fov
         binding.arView.fov = com.kylecorry.sol.math.geometry.Size(fov.first, fov.second)
-        binding.linearCompass.range = fov.first
 
         // Set the arView size to be the camera preview size
         val size = binding.camera.getPreviewSize()

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

@@ -3,10 +3,11 @@ package com.kylecorry.trail_sense.tools.augmented_reality
 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
 import android.view.MotionEvent
-import android.view.ScaleGestureDetector
 import com.kylecorry.andromeda.canvas.CanvasView
 import com.kylecorry.andromeda.canvas.TextAlign
 import com.kylecorry.andromeda.canvas.TextMode
@@ -16,6 +17,7 @@ import com.kylecorry.andromeda.sense.clinometer.CameraClinometer
 import com.kylecorry.andromeda.sense.clinometer.SideClinometer
 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
@@ -55,15 +57,21 @@ class AugmentedRealityView : CanvasView {
 
     var backgroundFillColor: Int = Color.TRANSPARENT
 
-    private var orientation = Quaternion.zero
-    private var inverseOrientation = Quaternion.zero
+    // 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 tempRotationResult = FloatArray(4)
+    private val tempWorldVector = FloatArray(4)
+
 
     // Sensors / preferences
     private val userPrefs = UserPreferences(context)
     private val sensors = SensorService(context)
     private val compass = sensors.getCompass()
     private val clinometer = CameraClinometer(context)
     private val sideClinometer = SideClinometer(context)
+    private val orientationSensor = sensors.getOrientation()
     private val gps = sensors.getGPS(frequency = Duration.ofMillis(200))
     private val altimeter = sensors.getAltimeter(gps = gps)
     private val declinationProvider = DeclinationFactory().getDeclinationStrategy(
@@ -72,6 +80,8 @@ class AugmentedRealityView : CanvasView {
     )
     private val isTrueNorth = userPrefs.compass.useTrueNorth
 
+    private val orientationLock = Any()
+
     /**
      * The compass bearing of the device in degrees.
      */
@@ -111,28 +121,33 @@ class AugmentedRealityView : CanvasView {
     private val layers = mutableListOf<ARLayer>()
     private val layerLock = Any()
 
-    private var useSensors = true
-    private val orientationLock = Any()
-
     // Guidance
     private var guideLocation: ARPosition? = null
     private var guideThreshold: Float? = null
     private var onGuideReached: (() -> Unit)? = null
 
     fun start() {
-        compass.start(this::onSensorUpdate)
-        clinometer.start(this::onSensorUpdate)
-        sideClinometer.start(this::onSensorUpdate)
         gps.start(this::onSensorUpdate)
         altimeter.start(this::onSensorUpdate)
+        if (orientationSensor != null) {
+            orientationSensor.start(this::onSensorUpdate)
+        } else {
+            compass.start(this::onSensorUpdate)
+            clinometer.start(this::onSensorUpdate)
+            sideClinometer.start(this::onSensorUpdate)
+        }
     }
 
     fun stop() {
-        compass.stop(this::onSensorUpdate)
-        clinometer.stop(this::onSensorUpdate)
-        sideClinometer.stop(this::onSensorUpdate)
         gps.stop(this::onSensorUpdate)
         altimeter.stop(this::onSensorUpdate)
+        if (orientationSensor != null) {
+            orientationSensor.stop(this::onSensorUpdate)
+        } else {
+            compass.stop(this::onSensorUpdate)
+            clinometer.stop(this::onSensorUpdate)
+            sideClinometer.stop(this::onSensorUpdate)
+        }
     }
 
     fun addLayer(layer: ARLayer) {
@@ -160,21 +175,6 @@ class AugmentedRealityView : CanvasView {
         }
     }
 
-    // TODO: Take in zoom
-    // TODO: Interpolate
-    /**
-     * Points the AR view at a coordinate
-     * @param coordinate The coordinate to point at
-     */
-    fun pointAt(coordinate: HorizonCoordinate) {
-        synchronized(orientationLock) {
-            useSensors = false
-            azimuth = getActualBearing(coordinate)
-            inclination = coordinate.elevation
-            sideInclination = 0f
-        }
-    }
-
     fun guideTo(
         location: ARPosition,
         thresholdDegrees: Float? = null,
@@ -191,30 +191,20 @@ class AugmentedRealityView : CanvasView {
         onGuideReached = null
     }
 
-    /**
-     * Sets the AR view to freeform mode
-     * @param isFreeform True if the view should be freeform, false otherwise (will use sensors)
-     */
-    fun setFreeform(isFreeform: Boolean) {
-        synchronized(orientationLock) {
-            useSensors = !isFreeform
-            sideInclination = 0f
-            // TODO: Allow dragging
-        }
-    }
-
     private fun onSensorUpdate(): Boolean {
+        if (orientationSensor != null) {
+            // No need to update the positions because the orientation sensor is valid
+            return true
+        }
         if (isTrueNorth) {
             compass.declination = declinationProvider.getDeclination()
         } else {
             compass.declination = 0f
         }
         synchronized(orientationLock) {
-            if (useSensors) {
-                azimuth = compass.rawBearing
-                inclination = clinometer.incline
-                sideInclination = sideClinometer.angle - 90f
-            }
+            azimuth = compass.rawBearing
+            inclination = clinometer.incline
+            sideInclination = sideClinometer.angle - 90f
         }
         return true
     }
@@ -350,7 +340,15 @@ class AugmentedRealityView : CanvasView {
     }
 
     private fun applyRotation(vector: Vector3): Vector3 {
-        return inverseOrientation.rotate(vector)
+        if (orientationSensor == null){
+            return legacyOrientation.rotate(vector)
+        }
+        tempWorldVector[0] = vector.x
+        tempWorldVector[1] = vector.y
+        tempWorldVector[2] = vector.z
+        tempWorldVector[3] = 1f
+        Matrix.multiplyMV(tempRotationResult, 0, rotationMatrix, 0, tempWorldVector, 0)
+        return Vector3(tempRotationResult[0], tempRotationResult[1], tempRotationResult[2])
     }
 
     private fun toSpherical(vector: Vector3): Vector3 {
@@ -431,8 +429,37 @@ class AugmentedRealityView : CanvasView {
     }
 
     private fun updateOrientation() {
-        orientation = Quaternion.from(Euler(inclination, -sideInclination, -azimuth))
-        inverseOrientation = orientation.inverse()
+        if (orientationSensor == null){
+            // TODO: This fails when the device is pointed almost straight up or down
+            legacyOrientation = Quaternion.from(Euler(inclination, -sideInclination, -azimuth)).inverse()
+            return
+        }
+
+
+        // Convert the orientation a rotation matrix
+        QuaternionMath.inverse(orientationSensor.rawOrientation, quaternion)
+        SensorManager.getRotationMatrixFromVector(rotationMatrix, quaternion)
+
+        // Remap the coordinate system to AR space
+        SensorManager.remapCoordinateSystem(
+            rotationMatrix,
+            SensorManager.AXIS_X,
+            SensorManager.AXIS_Z,
+            rotationMatrix
+        )
+
+        // Add declination
+        if (isTrueNorth) {
+            val declination = declinationProvider.getDeclination()
+            Matrix.rotateM(rotationMatrix, 0, declination, 0f, 0f, 1f)
+        }
+
+        // Get orientation from rotation matrix
+        val orientation = FloatArray(3)
+        SensorManager.getOrientation(rotationMatrix, orientation)
+        azimuth = orientation[0].toDegrees()
+        inclination = -orientation[1].toDegrees()
+        sideInclination = -orientation[2].toDegrees()
     }
 
     private val gestureListener = object : GestureDetector.SimpleOnGestureListener() {