Fix azimuth of rotation vector sensors

kylecorry31 · May 19, 2023 · 745715f · 745715f
1 parent 0838cee
commit 745715f
Show file tree

Hide file tree

Showing 7 changed files with 123 additions and 158 deletions.
diff --git a/build.gradle b/build.gradle
@@ -8,7 +8,7 @@ buildscript {
         classpath 'org.jetbrains.kotlin:kotlin-gradle-plugin:1.8.0'
     }
     ext.groupId = 'com.kylecorry.andromeda'
-    ext.versionName = '5.3.0'
+    ext.versionName = '5.3.1'
     ext.solVersion = '6.6.0'
     ext.targetVersion = 33
     ext.compileVersion = 33

diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/BaseRotationSensor.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/BaseRotationSensor.kt
@@ -0,0 +1,53 @@
+package com.kylecorry.andromeda.sense.orientation
+
+import android.content.Context
+import android.hardware.SensorEvent
+import android.hardware.SensorManager
+import com.kylecorry.andromeda.sense.BaseSensor
+import com.kylecorry.sol.math.Quaternion
+import com.kylecorry.sol.math.QuaternionMath
+
+abstract class BaseRotationSensor(context: Context, type: Int) :
+    BaseSensor(context, type, SensorManager.SENSOR_DELAY_FASTEST),
+    IOrientationSensor {
+
+    private val lock = Object()
+
+    override val hasValidReading: Boolean
+        get() = _hasReading
+
+    override val orientation: Quaternion
+        get() = Quaternion.from(rawOrientation)
+
+    override val rawOrientation: FloatArray
+        get() {
+            return synchronized(lock) {
+                _quaternion
+            }
+        }
+
+    private val _quaternion = Quaternion.zero.toFloatArray()
+
+    private var _hasReading = false
+
+    override fun handleSensorEvent(event: SensorEvent) {
+        synchronized(lock) {
+            onHandleSensorEvent(event)
+
+            // Calculate the quaternion
+            SensorManager.getQuaternionFromVector(_quaternion, event.values)
+            val w = _quaternion[0]
+            val x = _quaternion[1]
+            val y = _quaternion[2]
+            val z = _quaternion[3]
+            _quaternion[0] = x
+            _quaternion[1] = y
+            _quaternion[2] = z
+            _quaternion[3] = w
+            QuaternionMath.inverse(_quaternion, _quaternion)
+        }
+        _hasReading = true
+    }
+
+    protected open fun onHandleSensorEvent(event: SensorEvent) {}
+}
diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/BaseWorldRotationSensor.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/BaseWorldRotationSensor.kt
@@ -0,0 +1,36 @@
+package com.kylecorry.andromeda.sense.orientation
+
+import android.content.Context
+import android.hardware.SensorEvent
+import com.kylecorry.andromeda.sense.compass.ICompass
+import com.kylecorry.sol.units.Bearing
+
+abstract class BaseWorldRotationSensor(
+    context: Context,
+    private val useTrueNorth: Boolean,
+    type: Int
+) :
+    BaseRotationSensor(context, type), ICompass {
+
+    override val bearing: Bearing
+        get() = Bearing(rawBearing)
+
+    override var declination: Float = 0.0f
+
+    override val rawBearing: Float
+        get() {
+            return if (useTrueNorth) {
+                Bearing.getBearing(Bearing.getBearing(_bearing) + declination)
+            } else {
+                Bearing.getBearing(_bearing)
+            }
+        }
+
+    private val _orientation = OrientationCalculator()
+    private var _bearing = 0f
+
+    override fun onHandleSensorEvent(event: SensorEvent) {
+        super.onHandleSensorEvent(event)
+        _bearing = _orientation.getAzimuth(event.values)
+    }
+}
diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/GameRotationSensor.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/GameRotationSensor.kt
@@ -8,42 +8,4 @@ import com.kylecorry.sol.math.Quaternion
 import com.kylecorry.sol.math.QuaternionMath
 import com.kylecorry.andromeda.sense.BaseSensor
 
-class GameRotationSensor(context: Context) :
-    BaseSensor(context, Sensor.TYPE_GAME_ROTATION_VECTOR, SensorManager.SENSOR_DELAY_FASTEST),
-    IOrientationSensor {
-
-    private val lock = Object()
-
-    override val hasValidReading: Boolean
-        get() = _hasReading
-
-    override val orientation: Quaternion
-        get() = Quaternion.from(rawOrientation)
-
-    override val rawOrientation: FloatArray
-        get() {
-            return synchronized(lock) {
-                _quaternion
-            }
-        }
-
-    private val _quaternion = Quaternion.zero.toFloatArray()
-
-    private var _hasReading = false
-
-    override fun handleSensorEvent(event: SensorEvent) {
-        synchronized(lock) {
-            SensorManager.getQuaternionFromVector(_quaternion, event.values)
-            val w = _quaternion[0]
-            val x = _quaternion[1]
-            val y = _quaternion[2]
-            val z = _quaternion[3]
-            _quaternion[0] = x
-            _quaternion[1] = y
-            _quaternion[2] = z
-            _quaternion[3] = w
-            QuaternionMath.inverse(_quaternion, _quaternion)
-        }
-        _hasReading = true
-    }
-}
+class GameRotationSensor(context: Context) : BaseRotationSensor(context, Sensor.TYPE_GAME_ROTATION_VECTOR)
diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/GeomagneticRotationSensor.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/GeomagneticRotationSensor.kt
@@ -10,59 +10,5 @@ import com.kylecorry.sol.units.Bearing
 import com.kylecorry.andromeda.sense.BaseSensor
 import com.kylecorry.andromeda.sense.compass.ICompass
 
-class GeomagneticRotationSensor(context: Context, private val useTrueNorth: Boolean) :
-    BaseSensor(context, Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR, SensorManager.SENSOR_DELAY_FASTEST),
-    IOrientationSensor, ICompass {
-
-    private val lock = Object()
-
-    override val bearing: Bearing
-        get() = Bearing(rawBearing)
-
-    override var declination: Float = 0.0f
-
-    override val hasValidReading: Boolean
-        get() = _hasReading
-
-    override val rawBearing: Float
-        get(){
-            val euler = FloatArray(3)
-            QuaternionMath.toEuler(_quaternion, euler)
-            val yaw = euler[2]
-            return if (useTrueNorth) {
-                Bearing.getBearing(Bearing.getBearing(yaw) + declination)
-            } else {
-                Bearing.getBearing(yaw)
-            }
-        }
-
-    override val orientation: Quaternion
-        get() = Quaternion.from(rawOrientation)
-
-    override val rawOrientation: FloatArray
-        get() {
-            return synchronized(lock) {
-                _quaternion
-            }
-        }
-
-    private val _quaternion = Quaternion.zero.toFloatArray()
-
-    private var _hasReading = false
-
-    override fun handleSensorEvent(event: SensorEvent) {
-        synchronized(lock) {
-            SensorManager.getQuaternionFromVector(_quaternion, event.values)
-            val w = _quaternion[0]
-            val x = _quaternion[1]
-            val y = _quaternion[2]
-            val z = _quaternion[3]
-            _quaternion[0] = x
-            _quaternion[1] = y
-            _quaternion[2] = z
-            _quaternion[3] = w
-            QuaternionMath.inverse(_quaternion, _quaternion)
-        }
-        _hasReading = true
-    }
-}
+class GeomagneticRotationSensor(context: Context, useTrueNorth: Boolean) :
+    BaseWorldRotationSensor(context, useTrueNorth, Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR)
diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/OrientationCalculator.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/OrientationCalculator.kt
@@ -0,0 +1,28 @@
+package com.kylecorry.andromeda.sense.orientation
+
+import android.hardware.SensorManager
+import com.kylecorry.sol.math.SolMath.toDegrees
+
+internal class OrientationCalculator {
+
+    private val rotationMatrix = FloatArray(9)
+    private val remappedRotationMatrix = FloatArray(9)
+    private val orientation = FloatArray(3)
+
+    fun getAzimuth(rotation: FloatArray): Float = synchronized(this) {
+        if (rotation.size != 9) {
+            // It is a rotation vector
+            SensorManager.getRotationMatrixFromVector(rotationMatrix, rotation)
+            return getAzimuth(rotationMatrix)
+        }
+
+        SensorManager.remapCoordinateSystem(
+            rotation,
+            SensorManager.AXIS_Y,
+            SensorManager.AXIS_MINUS_X,
+            remappedRotationMatrix
+        )
+        SensorManager.getOrientation(remappedRotationMatrix, orientation)
+        return orientation[0].toDegrees() - 90f
+    }
+}
diff --git a/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/RotationSensor.kt b/sense/src/main/java/com/kylecorry/andromeda/sense/orientation/RotationSensor.kt
@@ -2,66 +2,6 @@ package com.kylecorry.andromeda.sense.orientation
 
 import android.content.Context
 import android.hardware.Sensor
-import android.hardware.SensorEvent
-import android.hardware.SensorManager
-import com.kylecorry.sol.math.Quaternion
-import com.kylecorry.sol.math.QuaternionMath
-import com.kylecorry.sol.units.Bearing
-import com.kylecorry.andromeda.sense.BaseSensor
-import com.kylecorry.andromeda.sense.compass.ICompass
 
-class RotationSensor(context: Context, private val useTrueNorth: Boolean) :
-    BaseSensor(context, Sensor.TYPE_ROTATION_VECTOR, SensorManager.SENSOR_DELAY_FASTEST),
-    IOrientationSensor, ICompass {
-
-    private val lock = Object()
-
-    override val bearing: Bearing
-        get() = Bearing(rawBearing)
-
-    override var declination: Float = 0.0f
-
-    override val hasValidReading: Boolean
-        get() = _hasReading
-    override val rawBearing: Float
-        get() {
-            val euler = FloatArray(3)
-            QuaternionMath.toEuler(_quaternion, euler)
-            val yaw = euler[2]
-            return if (useTrueNorth) {
-                Bearing.getBearing(Bearing.getBearing(yaw) + declination)
-            } else {
-                Bearing.getBearing(yaw)
-            }
-        }
-
-    override val orientation: Quaternion
-        get() = Quaternion.from(rawOrientation)
-
-    override val rawOrientation: FloatArray
-        get() {
-            return synchronized(lock) {
-                _quaternion
-            }
-        }
-
-    private val _quaternion = Quaternion.zero.toFloatArray()
-
-    private var _hasReading = false
-
-    override fun handleSensorEvent(event: SensorEvent) {
-        synchronized(lock) {
-            SensorManager.getQuaternionFromVector(_quaternion, event.values)
-            val w = _quaternion[0]
-            val x = _quaternion[1]
-            val y = _quaternion[2]
-            val z = _quaternion[3]
-            _quaternion[0] = x
-            _quaternion[1] = y
-            _quaternion[2] = z
-            _quaternion[3] = w
-            QuaternionMath.inverse(_quaternion, _quaternion)
-        }
-        _hasReading = true
-    }
-}
+class RotationSensor(context: Context, useTrueNorth: Boolean) :
+    BaseWorldRotationSensor(context, useTrueNorth, Sensor.TYPE_ROTATION_VECTOR)