Implement SixAxis sensor

app/src/main/java/emu/skyline/input/InputHandler.kt

@@ -5,17 +5,27 @@
 
 package emu.skyline.input
 
+import android.content.Context
+import android.hardware.Sensor
+import android.hardware.SensorEvent
+import android.hardware.SensorEventListener
+import android.hardware.SensorManager
 import android.view.InputDevice
 import android.view.KeyEvent
 import android.view.MotionEvent
 import android.view.View
+import androidx.core.content.getSystemService
+import emu.skyline.utils.ByteBufferSerializable
 import emu.skyline.utils.PreferenceSettings
+import emu.skyline.utils.u64
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
 import kotlin.math.abs
 
 /**
  * Handles input events during emulation
  */
-class InputHandler(private val inputManager : InputManager, private val preferenceSettings : PreferenceSettings) {
+class InputHandler(private val inputManager : InputManager, private val preferenceSettings : PreferenceSettings) : SensorEventListener {
     companion object {
         /**
          * This initializes a guest controller in libskyline
@@ -52,6 +62,15 @@ class InputHandler(private val inputManager : InputManager, private val preferen
          */
         external fun setAxisValue(index : Int, axis : Int, value : Int)
 
+        /**
+         * This sets the values of the points on the guest touch-screen
+         *
+         * @param index The index of the controller this is directed to
+         * @param motionId The ID of the motion sensor that is being modified
+         * @param value A byte buffer of skyline::input::MotionInput in C++
+         */
+        private external fun setMotionState(index : Int, motionId : Int, value : ByteBuffer)
+
         /**
          * This sets the values of the points on the guest touch-screen
          *
@@ -60,6 +79,20 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         external fun setTouchState(points : IntArray)
     }
 
+    data class MotionSensorInput(
+        var timestamp : u64 = 0uL,
+        var deltaTimestamp : u64 = 0uL,
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(3) var gyroscope : FloatArray = FloatArray(3),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(3) var accelerometer : FloatArray = FloatArray(3),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(4) var quaternion : FloatArray = FloatArray(4),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(9) var orientationMatrix : FloatArray = FloatArray(9),
+    ) : ByteBufferSerializable
+
+    /**
+     * The latest state of the motion sensor
+     */
+    private val motionSensor = MotionSensorInput()
+
     /**
      * Initializes all of the controllers from [InputManager] on the guest
      */
@@ -85,6 +118,28 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         updateControllers()
     }
 
+    fun initialiseMotionSensors(context : Context) {
+        val sensorManager = context.getSystemService<SensorManager>() ?: return
+        val sensorList = sensorManager.getSensorList(Sensor.TYPE_ALL)
+        val hasRotationVector = sensorList.any { sensor -> sensor.type == Sensor.TYPE_ROTATION_VECTOR }
+
+        sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER)?.also { accelerometer ->
+            sensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_GAME)
+        }
+        sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE)?.also { gyroscope ->
+            sensorManager.registerListener(this, gyroscope, SensorManager.SENSOR_DELAY_GAME)
+        }
+        sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR)?.also { rotationVector ->
+            sensorManager.registerListener(this, rotationVector, SensorManager.SENSOR_DELAY_GAME)
+        }
+        // Avoid listening to two rotation vectors at once
+        if (!hasRotationVector) {
+            sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR)?.also { rotationVector ->
+                sensorManager.registerListener(this, rotationVector, SensorManager.SENSOR_DELAY_GAME)
+            }
+        }
+    }
+
     /**
      * Handles translating any [KeyHostEvent]s to a [GuestEvent] that is passed into libskyline
      */
@@ -175,6 +230,56 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         return false
     }
 
+    override fun onAccuracyChanged(sensor : Sensor?, accuracy : Int) {}
+
+    /**
+     * This handles translating any [SensorEvent]s to a [GuestEvent] that is passed into libskyline
+     */
+    override fun onSensorChanged(event : SensorEvent) {
+        when (event.sensor.type) {
+            Sensor.TYPE_ACCELEROMETER -> {
+                motionSensor.accelerometer[0] = event.values[0] / SensorManager.GRAVITY_EARTH
+                motionSensor.accelerometer[1] = event.values[1] / SensorManager.GRAVITY_EARTH
+                motionSensor.accelerometer[2] = event.values[2] / SensorManager.GRAVITY_EARTH
+            }
+
+            Sensor.TYPE_GYROSCOPE -> {
+                // Investigate why sensor value is off by 12x
+                motionSensor.gyroscope[0] = event.values[0] / 12.0f
+                motionSensor.gyroscope[1] = event.values[1] / 12.0f
+                motionSensor.gyroscope[2] = event.values[2] / 12.0f
+            }
+
+            Sensor.TYPE_ROTATION_VECTOR -> {
+                motionSensor.quaternion[0] = event.values[0]
+                motionSensor.quaternion[1] = event.values[1]
+                motionSensor.quaternion[2] = event.values[2]
+                motionSensor.quaternion[3] = event.values[3]
+                SensorManager.getRotationMatrixFromVector(motionSensor.orientationMatrix, event.values)
+            }
+
+            Sensor.TYPE_GAME_ROTATION_VECTOR -> {
+                motionSensor.quaternion[0] = event.values[0]
+                motionSensor.quaternion[1] = event.values[1]
+                motionSensor.quaternion[2] = event.values[2]
+                motionSensor.quaternion[3] = event.values[3]
+                SensorManager.getRotationMatrixFromVector(motionSensor.orientationMatrix, event.values)
+            }
+
+            else -> {}
+        }
+
+        // Only update state on accelerometer data
+        if (event.sensor.type != Sensor.TYPE_ACCELEROMETER)
+            return
+
+        motionSensor.deltaTimestamp = event.timestamp.toULong() - motionSensor.timestamp
+        motionSensor.timestamp = event.timestamp.toULong()
+        setMotionState(0, 0, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+        setMotionState(0, 1, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+        setMotionState(0, 2, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+    }
+
     fun handleTouchEvent(view : View, event : MotionEvent) : Boolean {
         val count = event.pointerCount
         val points = IntArray(count * 7) // This is an array of skyline::input::TouchScreenPoint in C++ as that allows for efficient transfer of values to it