Implement support to sensors (#618)

* Implement sensors

* Fix memory leak in accel

include/sdl_sensors.hpp

@@ -2,31 +2,37 @@
 
 #include <cmath>
 #include <glm/glm.hpp>
-#include <numbers>
 
 #include "helpers.hpp"
 #include "services/hid.hpp"
 
+// Convert SDL sensor readings to 3DS format
+// We use the same code for Android as well, since the values we get from Android are in the same format as SDL (m/s^2 for acceleration, rad/s for
+// rotation)
 namespace Sensors::SDL {
-	// Convert the rotation data we get from SDL sensor events to rotation data we can feed right to HID
-	// Returns [pitch, roll, yaw]
-	static glm::vec3 convertRotation(glm::vec3 rotation) {
-		// Convert the rotation from rad/s to deg/s and scale by the gyroscope coefficient in HID
-		constexpr float scale = 180.f / std::numbers::pi * HIDService::gyroscopeCoeff;
-		// The axes are also inverted, so invert scale before the multiplication.
-		return rotation * -scale;
-	}
+    // Convert the rotation data we get from SDL sensor events to rotation data we can feed right to HID
+    // Returns [pitch, roll, yaw]
+    static glm::vec3 convertRotation(glm::vec3 rotation) {
+        // Annoyingly, Android doesn't support the <numbers> header yet so we define pi ourselves
+        static constexpr double pi = 3.141592653589793;
+        // Convert the rotation from rad/s to deg/s and scale by the gyroscope coefficient in HID
+        constexpr float scale = 180.f / pi * HIDService::gyroscopeCoeff;
+        // The axes are also inverted, so invert scale before the multiplication.
+        return rotation * -scale;
+    }
 
-	static glm::vec3 convertAcceleration(float* data) {
-		// Set our cap to ~9 m/s^2. The 3DS sensors cap at -930 and +930, so values above this value will get clamped to 930
-		// At rest (3DS laid flat on table), hardware reads around ~0 for x and z axis, and around ~480 for y axis due to gravity.
-		// This code tries to mimic this approximately, with offsets based on measurements from my DualShock 4.
-		static constexpr float accelMax = 9.f;
+    static glm::vec3 convertAcceleration(float* data) {
+        // Set our cap to ~9 m/s^2. The 3DS sensors cap at -930 and +930, so values above this value will get clamped to 930
+        // At rest (3DS laid flat on table), hardware reads around ~0 for x and z axis, and around ~480 for y axis due to gravity.
+        // This code tries to mimic this approximately, with offsets based on measurements from my DualShock 4.
+        static constexpr float accelMax = 9.f;
+        // We define standard gravity(g) ourself instead of using the SDL one in order for the code to work on Android too.
+        static constexpr float standardGravity = 9.80665f;
 
-		s16 x = std::clamp<s16>(s16(data[0] / accelMax * 930.f), -930, +930);
-		s16 y = std::clamp<s16>(s16(data[1] / (SDL_STANDARD_GRAVITY * accelMax) * 930.f - 350.f), -930, +930);
-		s16 z = std::clamp<s16>(s16((data[2] - 2.1f) / accelMax * 930.f), -930, +930);
+        s16 x = std::clamp<s16>(s16(data[0] / accelMax * 930.f), -930, +930);
+        s16 y = std::clamp<s16>(s16(data[1] / (standardGravity * accelMax) * 930.f - 350.f), -930, +930);
+        s16 z = std::clamp<s16>(s16((data[2] - 2.1f) / accelMax * 930.f), -930, +930);
 
-		return glm::vec3(x, y, z);
-	}
+        return glm::vec3(x, y, z);
+    }
 }  // namespace Sensors::SDL

src/jni_driver.cpp

@@ -8,6 +8,7 @@
 #include "renderer_gl/renderer_gl.hpp"
 #include "services/hid.hpp"
 #include "android_utils.hpp"
+#include "sdl_sensors.hpp"
 
 std::unique_ptr<Emulator> emulator = nullptr;
 HIDService* hidService = nullptr;
@@ -110,6 +111,19 @@ AlberFunction(void, TouchScreenUp)(JNIEnv* env, jobject obj) { hidService->relea
 AlberFunction(void, KeyUp)(JNIEnv* env, jobject obj, jint keyCode) { hidService->releaseKey((u32)keyCode); }
 AlberFunction(void, KeyDown)(JNIEnv* env, jobject obj, jint keyCode) { hidService->pressKey((u32)keyCode); }
 
+AlberFunction(void, SetGyro)(JNIEnv* env, jobject obj, jfloat roll, jfloat pitch, jfloat yaw) {
+    auto rotation = Sensors::SDL::convertRotation({ float(roll), float(pitch), float(yaw) });
+    hidService->setPitch(s16(rotation.x));
+    hidService->setRoll(s16(rotation.y));
+    hidService->setYaw(s16(rotation.z));
+}
+
+AlberFunction(void, SetAccel)(JNIEnv* env, jobject obj, jfloat rawX, jfloat rawY, jfloat rawZ) {
+    float data[3] = { float(rawX), float(rawY), float(rawZ) };
+    auto accel = Sensors::SDL::convertAcceleration(data);
+    hidService->setAccel(accel.x, accel.y, accel.z);
+}
+
 AlberFunction(void, SetCirclepadAxis)(JNIEnv* env, jobject obj, jint x, jint y) {
 	hidService->setCirclepadX((s16)x);
 	hidService->setCirclepadY((s16)y);
@@ -139,4 +153,4 @@ int AndroidUtils::openDocument(const char* path, const char* perms) {
     env->DeleteLocalRef(jmode);
 
     return (int)result;
-}
+}

src/pandroid/app/src/main/java/com/panda3ds/pandroid/AlberDriver.java

@@ -24,7 +24,9 @@ public class AlberDriver {
 	public static native void KeyUp(int code);
 	public static native void SetCirclepadAxis(int x, int y);
 	public static native void TouchScreenUp();
-	public static native void TouchScreenDown(int x, int y);
+	public static native void TouchScreenDown(int x, int y);;
+	public static native void SetGyro(float roll, float pitch, float yaw);
+	public static native void SetAccel(float x, float y, float z);
 	public static native void Pause();
 	public static native void Resume();
 	public static native void LoadLuaScript(String script);

src/pandroid/app/src/main/java/com/panda3ds/pandroid/app/GameActivity.java

@@ -3,11 +3,21 @@ package com.panda3ds.pandroid.app;
 import android.app.ActivityManager;
 import android.app.PictureInPictureParams;
 import android.content.Intent;
+import android.content.res.Configuration;
+import android.hardware.Sensor;
+import android.hardware.SensorEvent;
+import android.hardware.SensorEventListener;
+import android.hardware.SensorManager;
+import android.opengl.Matrix;
 import android.os.Build;
 import android.os.Bundle;
+import android.renderscript.Matrix3f;
+import android.renderscript.Matrix4f;
+import android.util.Log;
 import android.util.Rational;
 import android.view.KeyEvent;
 import android.view.MotionEvent;
+import android.view.Surface;
 import android.view.View;
 import android.view.ViewGroup;
 import android.view.WindowManager;
@@ -25,6 +35,7 @@ import com.panda3ds.pandroid.app.game.EmulatorCallback;
 import com.panda3ds.pandroid.data.config.GlobalConfig;
 import com.panda3ds.pandroid.input.InputHandler;
 import com.panda3ds.pandroid.input.InputMap;
+import com.panda3ds.pandroid.math.Vector3;
 import com.panda3ds.pandroid.utils.Constants;
 import com.panda3ds.pandroid.view.PandaGlSurfaceView;
 import com.panda3ds.pandroid.view.PandaLayoutController;
@@ -32,7 +43,7 @@ import com.panda3ds.pandroid.view.ds.DsLayoutManager;
 import com.panda3ds.pandroid.view.renderer.ConsoleRenderer;
 import com.panda3ds.pandroid.view.utils.PerformanceView;
 
-public class GameActivity extends BaseActivity implements EmulatorCallback {
+public class GameActivity extends BaseActivity implements EmulatorCallback, SensorEventListener {
 	private final DrawerFragment drawerFragment = new DrawerFragment();
 	private final AlberInputListener inputListener = new AlberInputListener(this);
 	private ConsoleRenderer renderer;
@@ -74,6 +85,19 @@ public class GameActivity extends BaseActivity implements EmulatorCallback {
 			((FrameLayout) findViewById(R.id.panda_gl_frame)).addView(view, new FrameLayout.LayoutParams(ViewGroup.LayoutParams.WRAP_CONTENT, ViewGroup.LayoutParams.WRAP_CONTENT));
 		}
 		swapScreens(GlobalConfig.get(GlobalConfig.KEY_CURRENT_DS_LAYOUT));
+		registerSensors();
+	}
+
+	private void registerSensors() {
+		SensorManager sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
+		Sensor accel = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+		if (accel != null) {
+			sensorManager.registerListener(this, accel, 1);
+		}
+		Sensor gryro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
+		if (gryro != null) {
+			sensorManager.registerListener(this, gryro, 1);
+		}
 	}
 
 	private void changeOverlayVisibility(boolean visible) {
@@ -94,6 +118,7 @@ public class GameActivity extends BaseActivity implements EmulatorCallback {
 		if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.O_MR1) {
 			getTheme().applyStyle(R.style.GameActivityNavigationBar, true);
 		}
+		registerSensors();
 	}
 
 	private void enablePIP() {
@@ -113,6 +138,7 @@ public class GameActivity extends BaseActivity implements EmulatorCallback {
 	protected void onPause() {
 		super.onPause();
 
+		((SensorManager)getSystemService(SENSOR_SERVICE)).unregisterListener(this);
 		InputHandler.reset();
 		if (GlobalConfig.get(GlobalConfig.KEY_PICTURE_IN_PICTURE)) {
 			if (Build.VERSION.SDK_INT > Build.VERSION_CODES.O) {
@@ -174,10 +200,45 @@ public class GameActivity extends BaseActivity implements EmulatorCallback {
 
 	@Override
 	protected void onDestroy() {
+		((SensorManager)getSystemService(SENSOR_SERVICE)).unregisterListener(this);
 		if (AlberDriver.HasRomLoaded()) {
 			AlberDriver.Finalize();
 		}
 
 		super.onDestroy();
 	}
+
+	private float getDeviceRotationAngle() {
+		int rotation = getWindow().getDecorView().getDisplay().getRotation();
+		switch (rotation) {
+			case Surface.ROTATION_90: return 90.0f;
+			case Surface.ROTATION_180: return 180.0f;
+			case Surface.ROTATION_270: return -90.0f;
+			default: return 0.0f;
+		}
+	}
+
+	@Override
+	public void onSensorChanged(SensorEvent event) {
+		if (AlberDriver.HasRomLoaded()) {
+			Sensor sensor = event.sensor;
+			switch (sensor.getType()) {
+				case Sensor.TYPE_ACCELEROMETER: {
+					float[] values = event.values;
+					Vector3 vec3 = new Vector3(values[0], values[1], values[2]);
+					vec3.rotateByEuler(new Vector3(0, 0, (float) (getDeviceRotationAngle() * (Math.PI / 180.0f))));
+					AlberDriver.SetAccel(vec3.x, vec3.y, vec3.z);
+				} break;
+				case Sensor.TYPE_GYROSCOPE: {
+					float[] values = event.values;
+					Vector3 vec3 = new Vector3(values[0], values[1], values[2]);
+					vec3.rotateByEuler(new Vector3(0, 0, (float) (getDeviceRotationAngle() * (Math.PI / 180.0f))));
+					AlberDriver.SetGyro(vec3.x, vec3.y, vec3.z);
+				} break;
+			}
+		}
+	}
+
+	@Override
+	public void onAccuracyChanged(Sensor sensor, int accuracy) {}
 }

src/pandroid/app/src/main/java/com/panda3ds/pandroid/app/provider/AppDataDocumentProvider.java

@@ -95,7 +95,7 @@ public class AppDataDocumentProvider extends DocumentsProvider {
     private void includeFile(MatrixCursor cursor, File file) {
         int flags = 0;
         if (file.isDirectory()) {
-            flags = Document.FLAG_DIR_SUPPORTS_CREATE;
+            flags = Document.FLAG_DIR_SUPPORTS_CREATE | Document.FLAG_SUPPORTS_DELETE;
         } else {
             flags = Document.FLAG_SUPPORTS_WRITE | Document.FLAG_SUPPORTS_REMOVE | Document.FLAG_SUPPORTS_DELETE;
         }

src/pandroid/app/src/main/java/com/panda3ds/pandroid/math/Quaternion.java

@@ -0,0 +1,31 @@
+package com.panda3ds.pandroid.math;
+
+public class Quaternion {
+	public float x, y, z, w;
+	public Quaternion(float x, float y, float z, float w) {
+		this.x = x;
+		this.y = y;
+		this.z = z;
+		this.w = w;
+	}
+
+	public Quaternion fromEuler(Vector3 euler) {
+		float x = euler.x;
+		float y = euler.y;
+		float z = euler.z;
+
+		double c1 = Math.cos(x / 2.0);
+		double c2 = Math.cos(y / 2.0);
+		double c3 = Math.cos(z / 2.0);
+
+		double s1 = Math.sin(x / 2.0);
+		double s2 = Math.sin(y / 2.0);
+		double s3 = Math.sin(z / 2.0);
+
+		this.x = (float) (s1 * c2 * c3 + c1 * s2 * s3);
+		this.y = (float) (c1 * s2 * c3 - s1 * c2 * s3);
+		this.z = (float) (c1 * c2 * s3 + s1 * s2 * c3);
+		this.w = (float) (c1 * c2 * c3 - s1 * s2 * s3);
+		return this;
+	}
+}

src/pandroid/app/src/main/java/com/panda3ds/pandroid/math/Vector3.java

@@ -0,0 +1,32 @@
+package com.panda3ds.pandroid.math;
+
+public class Vector3 {
+	private final Quaternion quaternion = new Quaternion(0, 0, 0, 0);
+	public float x, y, z;
+
+	public Vector3(float x, float y, float z) {
+		this.x = x;
+		this.y = y;
+		this.z = z;
+	}
+
+	public Vector3 rotateByEuler(Vector3 euler) {
+		this.quaternion.fromEuler(euler);
+
+		float x = this.x, y = this.y, z = this.z;
+		float qx = this.quaternion.x;
+		float qy = this.quaternion.y;
+		float qz = this.quaternion.z;
+		float qw = this.quaternion.w;
+
+		float ix = qw * x + qy * z - qz * y;
+		float iy = qw * y + qz * x - qx * z;
+		float iz = qw * z + qx * y - qy * x;
+		float iw = -qx * x - qy * qz * z;
+
+		this.x = ix * qw + iw * -qx + iy * -qz - iz * -qy;
+		this.y = iy * qw + iw * -qy + iz * -qx - ix * -qz;
+		this.z = iz * qw + iw * -qz + ix * -qy - iy * -qx;
+		return this;
+	}
+}