1  using Ryujinx.Input.Motion;
 2  using System;
 3  using System.Numerics;
 4  
 5  namespace Ryujinx.Input
 6  {
 7      public class MotionInput
 8      {
 9          public ulong TimeStamp { get; set; }
10          public Vector3 Accelerometer { get; set; }
11          public Vector3 Gyroscrope { get; set; }
12          public Vector3 Rotation { get; set; }
13  
14          private readonly MotionSensorFilter _filter;
15  
16          public MotionInput()
17          {
18              TimeStamp = 0;
19              Accelerometer = new Vector3();
20              Gyroscrope = new Vector3();
21              Rotation = new Vector3();
22  
23              // TODO: RE the correct filter.
24              _filter = new MotionSensorFilter(0f);
25          }
26  
27          public void Update(Vector3 accel, Vector3 gyro, ulong timestamp, int sensitivity, float deadzone)
28          {
29              if (TimeStamp != 0)
30              {
31                  Accelerometer = -accel;
32  
33                  if (gyro.Length() < deadzone)
34                  {
35                      gyro = Vector3.Zero;
36                  }
37  
38                  gyro *= (sensitivity / 100f);
39  
40                  Gyroscrope = gyro;
41  
42                  float deltaTime = MathF.Abs((long)(timestamp - TimeStamp) / 1000000f);
43  
44                  Vector3 deltaGyro = gyro * deltaTime;
45  
46                  Rotation += deltaGyro;
47  
48                  _filter.SamplePeriod = deltaTime;
49                  _filter.Update(accel, DegreeToRad(gyro));
50              }
51  
52              TimeStamp = timestamp;
53          }
54  
55          public Matrix4x4 GetOrientation()
56          {
57              return Matrix4x4.CreateFromQuaternion(_filter.Quaternion);
58          }
59  
60          private static Vector3 DegreeToRad(Vector3 degree)
61          {
62              return degree * (MathF.PI / 180);
63          }
64      }
65  }