diff --git a/BetterJoyForCemu/BetterJoyForCemu.csproj b/BetterJoyForCemu/BetterJoyForCemu.csproj index 27c0098..d7c9bf1 100644 --- a/BetterJoyForCemu/BetterJoyForCemu.csproj +++ b/BetterJoyForCemu/BetterJoyForCemu.csproj @@ -88,7 +88,6 @@ - diff --git a/BetterJoyForCemu/Joycon.cs b/BetterJoyForCemu/Joycon.cs index f39e9ab..c4771ed 100644 --- a/BetterJoyForCemu/Joycon.cs +++ b/BetterJoyForCemu/Joycon.cs @@ -13,6 +13,7 @@ namespace BetterJoyForCemu { float timing = 60.0f; bool isPro = false; + bool isUSB = false; public enum DebugType : int { NONE, @@ -219,7 +220,7 @@ namespace BetterJoyForCemu { public int packetCounter = 0; // - public Joycon(IntPtr handle_, bool imu, bool localize, float alpha, bool left, int id = 0, bool isPro=false) { + public Joycon(IntPtr handle_, bool imu, bool localize, float alpha, bool left, int id = 0, bool isPro=false, bool usb = false) { handle = handle_; imu_enabled = imu; do_localize = localize; @@ -229,6 +230,7 @@ namespace BetterJoyForCemu { PadId = id; this.isPro = isPro; + isUSB = usb; } public void DebugPrint(String s, DebugType d) { if (debug_type == DebugType.NONE) return; @@ -257,42 +259,72 @@ namespace BetterJoyForCemu { public Vector3 GetAccel() { return acc_g; } - public Quaternion GetVector() { - Vector3 v1 = new Vector3(j_b.X, i_b.X, k_b.X); - Vector3 v2 = -(new Vector3(j_b.Z, i_b.Z, k_b.Z)); - if (v2 != Vector3.Zero) { - MyQuaternion temp = MyQuaternion.LookRotation(v1, v2); - return new Quaternion(temp.eulerAngles, temp.Length); - } else { - return Quaternion.Identity; - } - } public int Attach(byte leds_ = 0x0) { state = state_.ATTACHED; + + // Make sure command is received + HIDapi.hid_set_nonblocking(handle, 0); + byte[] a = { 0x0 }; - // Input report mode - Subcommand(0x3, new byte[] { 0x30 }, 1, false); - Subcommand(0x3, new byte[] { 0x03, 0x00, 0x00, 0x01 }, 4, false); // higher gyro performance rate - a[0] = 0x1; - dump_calibration_data(); + // Connect - a[0] = 0x01; - Subcommand(0x1, a, 1); - a[0] = 0x02; - Subcommand(0x1, a, 1); - a[0] = 0x03; - Subcommand(0x1, a, 1); + if (!isUSB) { + // Input report mode + Subcommand(0x03, new byte[] { 0x30 }, 1, false); + a[0] = 0x1; + dump_calibration_data(); + } else { + Subcommand(0x03, new byte[] { 0x3f }, 1, false); + + a = Enumerable.Repeat((byte)0, 64).ToArray(); + Console.WriteLine("Using USB."); + + a[0] = 0x80; + a[1] = 0x01; + HIDapi.hid_write(handle, a, new UIntPtr(2)); + HIDapi.hid_read(handle, a, new UIntPtr(64)); + + if (a[2] != 0x3) { + PadMacAddress = new PhysicalAddress(new byte[] { a[9], a[8], a[7], a[6], a[5], a[4] }); + } + + // USB Pairing + a = Enumerable.Repeat((byte)0, 64).ToArray(); + a[0] = 0x80; a[1] = 0x02; // Handshake + HIDapi.hid_write(handle, a, new UIntPtr(2)); + + a[0] = 0x80; a[1] = 0x03; // 3Mbit baud rate + HIDapi.hid_write(handle, a, new UIntPtr(2)); + + a[0] = 0x80; a[1] = 0x02; // Handshake at new baud rate + HIDapi.hid_write(handle, a, new UIntPtr(2)); + + a[0] = 0x80; a[1] = 0x04; // Prevent HID timeout + HIDapi.hid_write(handle, a, new UIntPtr(2)); + + dump_calibration_data(); + } + a[0] = leds_; Subcommand(0x30, a, 1); Subcommand(0x40, new byte[] { (imu_enabled ? (byte)0x1 : (byte)0x0) }, 1, true); Subcommand(0x3, new byte[] { 0x30 }, 1, true); Subcommand(0x48, new byte[] { 0x1 }, 1, true); + + if (!isUSB) + Subcommand(0x41, new byte[] { 0x03, 0x00, 0x00, 0x01 }, 4, false); // higher gyro performance rate + DebugPrint("Done with init.", DebugType.COMMS); + + HIDapi.hid_set_nonblocking(handle, 1); + return 0; } + public void SetFilterCoeff(float a) { filterweight = a; } + public void Detach() { stop_polling = true; PrintArray(max, format: "Max {0:S}", d: DebugType.IMU); @@ -301,6 +333,13 @@ namespace BetterJoyForCemu { //Subcommand(0x30, new byte[] { 0x0 }, 1); // Turn off LEDS after pair Subcommand(0x40, new byte[] { 0x0 }, 1); Subcommand(0x48, new byte[] { 0x0 }, 1); + + if (isUSB) { + byte[] a = Enumerable.Repeat((byte)0, 64).ToArray(); + a[0] = 0x80; a[1] = 0x05; // Allow device to talk to BT again + HIDapi.hid_write(handle, a, new UIntPtr(2)); + } + //Subcommand(0x3, new byte[] { 0x3f }, 1); // Turn on basic HID mode - not needed } if (state > state_.DROPPED) { @@ -308,6 +347,7 @@ namespace BetterJoyForCemu { } state = state_.NOT_ATTACHED; } + private byte ts_en; private byte ts_de; private System.DateTime ts_prev; @@ -328,6 +368,7 @@ namespace BetterJoyForCemu { } return ret; } + private Thread PollThreadObj; private void Poll() { int attempts = 0; @@ -362,23 +403,19 @@ namespace BetterJoyForCemu { rep.CopyBuffer(report_buf); } if (imu_enabled) { - if (do_localize) { - ProcessIMU(report_buf); - } else { - ExtractIMUValues(report_buf, 0); - // 3 values for 5ms precision instead of 15ms - /*for (int n = 0; n < 3; n++) { - ExtractIMUValues(report_buf, n); + ExtractIMUValues(report_buf, 0); + // 3 values for 5ms precision instead of 15ms + /*for (int n = 0; n < 3; n++) { + ExtractIMUValues(report_buf, n); - Timestamp = rep.ts + (ulong) n * 5000; // 5ms difference + Timestamp = rep.ts + (ulong) n * 5000; // 5ms difference - if (n == 0) - ProcessButtonsAndStick(report_buf); + if (n == 0) + ProcessButtonsAndStick(report_buf); - packetCounter++; - Program.server.NewReportIncoming(this); - }*/ - } + packetCounter++; + Program.server.NewReportIncoming(this); + }*/ } if (ts_de == report_buf[1]) { DebugPrint(string.Format("Duplicate timestamp dequeued. TS: {0:X2}", ts_de), DebugType.THREADING); @@ -483,90 +520,27 @@ namespace BetterJoyForCemu { else offset = right_hor_offset; - //Console.WriteLine("{0} {1} {2}", gyr_r[0], gyr_r[1], gyr_r[2]); - for (int i = 0; i < 3; ++i) { switch (i) { case 0: acc_g.X = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; - gyr_g.X = gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); + gyr_g.X = (gyr_r[i] - gyr_neutral[i]) * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); break; case 1: acc_g.Y = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; - gyr_g.Y = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); + gyr_g.Y = -(gyr_r[i] - gyr_neutral[i] * 0.5f) * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); break; case 2: acc_g.Z = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; - gyr_g.Z = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); + gyr_g.Z = -(gyr_r[i] - gyr_neutral[i]) * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])); break; } } } - private float err; - public Vector3 i_b, j_b, k_b, k_acc; - private Vector3 d_theta; - private Vector3 i_b_; - private Vector3 w_a, w_g; - private Quaternion vec; - - private int ProcessIMU(byte[] report_buf) { - - // Direction Cosine Matrix method - // http://www.starlino.com/dcm_tutorial.html - - if (!imu_enabled | state < state_.IMU_DATA_OK) - return -1; - - if (report_buf[0] != 0x30) return -1; // no gyro data - - // read raw IMU values - int dt = (report_buf[1] - timestamp); - if (report_buf[1] < timestamp) dt += 0x100; - - for (int n = 0; n < 3; ++n) { - ExtractIMUValues(report_buf, n); - - float dt_sec = 0.005f * dt; - sum[0] += gyr_g.X * dt_sec; - sum[1] += gyr_g.Y * dt_sec; - sum[2] += gyr_g.Z * dt_sec; - - if (isLeft && !isPro) { // not sure about this - gyr_g.Y *= -1; - gyr_g.Z *= -1; - acc_g.Y *= -1; - acc_g.Z *= -1; - } - - if (first_imu_packet) { - i_b = new Vector3(1, 0, 0); - j_b = new Vector3(0, 1, 0); - k_b = new Vector3(0, 0, 1); - first_imu_packet = false; - } else { - k_acc = -Vector3.Normalize(acc_g); - w_a = Vector3.Cross(k_b, k_acc); - w_g = -gyr_g * dt_sec; - d_theta = (filterweight * w_a + w_g) / (1f + filterweight); - k_b += Vector3.Cross(d_theta, k_b); - i_b += Vector3.Cross(d_theta, i_b); - j_b += Vector3.Cross(d_theta, j_b); - //Correction, ensure new axes are orthogonal - err = Vector3.Dot(i_b, j_b) * 0.5f; - i_b_ = Vector3.Normalize(i_b - err * j_b); - j_b = Vector3.Normalize(j_b - err * i_b); - i_b = i_b_; - k_b = Vector3.Cross(i_b, j_b); - } - dt = 1; - } - timestamp = report_buf[1] + 2; - return 0; - } public void Begin() { if (PollThreadObj == null) { PollThreadObj = new Thread(new ThreadStart(Poll)); @@ -575,10 +549,12 @@ namespace BetterJoyForCemu { Console.WriteLine("Starting poll thread."); } } + public void Recenter() { first_imu_packet = true; } - private float[] CenterSticks(UInt16[] vals, bool special=false) { + + private float[] CenterSticks(UInt16[] vals, bool special = false) { ushort[] t = stick_cal; if (special) @@ -597,12 +573,14 @@ namespace BetterJoyForCemu { } return s; } + public void SetRumble(float low_freq, float high_freq, float amp, int time = 0) { if (state <= Joycon.state_.ATTACHED) return; if (rumble_obj.timed_rumble == false || rumble_obj.t < 0) { rumble_obj = new Rumble(low_freq, high_freq, amp, time); } } + private void SendRumble(byte[] buf) { byte[] buf_ = new byte[report_len]; buf_[0] = 0x10; @@ -613,6 +591,7 @@ namespace BetterJoyForCemu { PrintArray(buf_, DebugType.RUMBLE, format: "Rumble data sent: {0:S}"); HIDapi.hid_write(handle, buf_, new UIntPtr(report_len)); } + private byte[] Subcommand(byte sc, byte[] buf, uint len, bool print = true) { byte[] buf_ = new byte[report_len]; byte[] response = new byte[report_len]; @@ -630,6 +609,7 @@ namespace BetterJoyForCemu { else if (print) { PrintArray(response, DebugType.COMMS, report_len - 1, 1, "Response ID 0x" + string.Format("{0:X2}", response[0]) + ". Data: 0x{0:S}"); } return response; } + private void dump_calibration_data() { byte[] buf_ = ReadSPI(0x80, (isLeft ? (byte)0x12 : (byte)0x1d), 9); // get user calibration data if possible bool found = false; @@ -730,6 +710,7 @@ namespace BetterJoyForCemu { PrintArray(gyr_neutral, len: 3, d: DebugType.IMU, format: "Factory gyro neutral position: {0:S}"); } } + private byte[] ReadSPI(byte addr1, byte addr2, uint len, bool print = false) { byte[] buf = { addr2, addr1, 0x00, 0x00, (byte)len }; byte[] read_buf = new byte[len]; @@ -745,6 +726,7 @@ namespace BetterJoyForCemu { if (print) PrintArray(read_buf, DebugType.COMMS, len); return read_buf; } + private void PrintArray(T[] arr, DebugType d = DebugType.NONE, uint len = 0, uint start = 0, string format = "{0:S}") { if (d != debug_type && debug_type != DebugType.ALL) return; if (len == 0) len = (uint)arr.Length; diff --git a/BetterJoyForCemu/MyQuaternion.cs b/BetterJoyForCemu/MyQuaternion.cs deleted file mode 100644 index ce6956a..0000000 --- a/BetterJoyForCemu/MyQuaternion.cs +++ /dev/null @@ -1,675 +0,0 @@ -using System; -using System.Runtime.Serialization; -using System.Xml.Serialization; -using System.Numerics; - -/// -/// Quaternions are used to represent rotations. -/// A custom completely managed implementation of UnityEngine.Quaternion -/// Base is decompiled UnityEngine.Quaternion -/// Doesn't implement methods marked Obsolete -/// Does implicit coversions to and from UnityEngine.Quaternion -/// -/// Uses code from: -/// https://raw.githubusercontent.com/mono/opentk/master/Source/OpenTK/Math/Quaternion.cs -/// http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html -/// http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine -/// http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha -/// -/// Version: aeroson 2017-07-11 (author yyyy-MM-dd) -/// License: ODC Public Domain Dedication & License 1.0 (PDDL-1.0) https://tldrlegal.com/license/odc-public-domain-dedication-&-license-1.0-(pddl-1.0) -/// -[Serializable] -public struct MyQuaternion : IEquatable { - const float radToDeg = (float)(180.0 / Math.PI); - const float degToRad = (float)(Math.PI / 180.0); - - public const float kEpsilon = 1E-06f; // should probably be used in the 0 tests in LookRotation or Slerp - - [XmlIgnore] - public Vector3 xyz { - set { - x = value.X; - y = value.Y; - z = value.Z; - } - get { - return new Vector3(x, y, z); - } - } - /// - /// X component of the Quaternion. Don't modify this directly unless you know quaternions inside out. - /// - public float x; - /// - /// Y component of the Quaternion. Don't modify this directly unless you know quaternions inside out. - /// - public float y; - /// - /// Z component of the Quaternion. Don't modify this directly unless you know quaternions inside out. - /// - public float z; - /// - /// W component of the Quaternion. Don't modify this directly unless you know quaternions inside out. - /// - public float w; - - [XmlIgnore] - public float this[int index] { - get { - switch (index) { - case 0: - return this.x; - case 1: - return this.y; - case 2: - return this.z; - case 3: - return this.w; - default: - throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3"); - } - } - set { - switch (index) { - case 0: - this.x = value; - break; - case 1: - this.y = value; - break; - case 2: - this.z = value; - break; - case 3: - this.w = value; - break; - default: - throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3"); - } - } - } - /// - /// The identity rotation (RO). - /// - [XmlIgnore] - public static MyQuaternion identity { - get { - return new MyQuaternion(0f, 0f, 0f, 1f); - } - } - /// - /// Returns the euler angle representation of the rotation. - /// - [XmlIgnore] - public Vector3 eulerAngles { - get { - return MyQuaternion.ToEulerRad(this) * radToDeg; - } - set { - this = MyQuaternion.FromEulerRad(value * degToRad); - } - } - /// - /// Gets the length (magnitude) of the quaternion. - /// - /// - [XmlIgnore] - public float Length { - get { - return (float)System.Math.Sqrt(x * x + y * y + z * z + w * w); - } - } - - /// - /// Gets the square of the quaternion length (magnitude). - /// - [XmlIgnore] - public float LengthSquared { - get { - return x * x + y * y + z * z + w * w; - } - } - /// - /// Constructs new MyQuaternion with given x,y,z,w components. - /// - /// - /// - /// - /// - public MyQuaternion(float x, float y, float z, float w) { - this.x = x; - this.y = y; - this.z = z; - this.w = w; - } - /// - /// Construct a new MyQuaternion from vector and w components - /// - /// The vector part - /// The w part - public MyQuaternion(Vector3 v, float w) { - this.x = v.X; - this.y = v.Y; - this.z = v.Z; - this.w = w; - } - /// - /// Set x, y, z and w components of an existing MyQuaternion. - /// - /// - /// - /// - /// - public void Set(float new_x, float new_y, float new_z, float new_w) { - this.x = new_x; - this.y = new_y; - this.z = new_z; - this.w = new_w; - } - /// - /// Scales the MyQuaternion to unit length. - /// - public void Normalize() { - float scale = 1.0f / this.Length; - xyz *= scale; - w *= scale; - } - /// - /// Scale the given quaternion to unit length - /// - /// The quaternion to normalize - /// The normalized quaternion - public static MyQuaternion Normalize(MyQuaternion q) { - MyQuaternion result; - Normalize(ref q, out result); - return result; - } - /// - /// Scale the given quaternion to unit length - /// - /// The quaternion to normalize - /// The normalized quaternion - public static void Normalize(ref MyQuaternion q, out MyQuaternion result) { - float scale = 1.0f / q.Length; - result = new MyQuaternion(q.xyz * scale, q.w * scale); - } - /// - /// The dot product between two rotations. - /// - /// - /// - public static float Dot(MyQuaternion a, MyQuaternion b) { - return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; - } - /// - /// Creates a rotation which rotates /angle/ degrees around /axis/. - /// - /// - /// - public static MyQuaternion AngleAxis(float angle, Vector3 axis) { - return MyQuaternion.AngleAxis(angle, ref axis); - } - private static MyQuaternion AngleAxis(float degress, ref Vector3 axis) { - if (axis.LengthSquared() == 0.0f) - return identity; - - MyQuaternion result = identity; - var radians = degress * degToRad; - radians *= 0.5f; - axis = Vector3.Normalize(axis); - axis = axis * (float)System.Math.Sin(radians); - result.x = axis.X; - result.y = axis.Y; - result.z = axis.Z; - result.w = (float)System.Math.Cos(radians); - - return Normalize(result); - } - public void ToAngleAxis(out float angle, out Vector3 axis) { - MyQuaternion.ToAxisAngleRad(this, out axis, out angle); - angle *= radToDeg; - } - /// - /// Creates a rotation which rotates from /fromDirection/ to /toDirection/. - /// - /// - /// - public static MyQuaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection) { - return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), float.MaxValue); - } - /// - /// Creates a rotation which rotates from /fromDirection/ to /toDirection/. - /// - /// - /// - public void SetFromToRotation(Vector3 fromDirection, Vector3 toDirection) { - this = MyQuaternion.FromToRotation(fromDirection, toDirection); - } - /// - /// Creates a rotation with the specified /forward/ and /upwards/ directions. - /// - /// The direction to look in. - /// The vector that defines in which direction up is. - public static MyQuaternion LookRotation(Vector3 forward, Vector3 upwards) { - return MyQuaternion.LookRotation(ref forward, ref upwards); - } - public static MyQuaternion LookRotation(Vector3 forward) { - Vector3 up = new Vector3(0, 0, 1); - return MyQuaternion.LookRotation(ref forward, ref up); - } - // from http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html - private static MyQuaternion LookRotation(ref Vector3 forward, ref Vector3 up) { - - forward = Vector3.Normalize(forward); - Vector3 right = Vector3.Normalize(Vector3.Cross(up, forward)); - up = Vector3.Cross(forward, right); - var m00 = right.X; - var m01 = right.Y; - var m02 = right.Z; - var m10 = up.X; - var m11 = up.Y; - var m12 = up.Z; - var m20 = forward.X; - var m21 = forward.Y; - var m22 = forward.Z; - - - float num8 = (m00 + m11) + m22; - var quaternion = new MyQuaternion(); - if (num8 > 0f) { - var num = (float)System.Math.Sqrt(num8 + 1f); - quaternion.w = num * 0.5f; - num = 0.5f / num; - quaternion.x = (m12 - m21) * num; - quaternion.y = (m20 - m02) * num; - quaternion.z = (m01 - m10) * num; - return quaternion; - } - if ((m00 >= m11) && (m00 >= m22)) { - var num7 = (float)System.Math.Sqrt(((1f + m00) - m11) - m22); - var num4 = 0.5f / num7; - quaternion.x = 0.5f * num7; - quaternion.y = (m01 + m10) * num4; - quaternion.z = (m02 + m20) * num4; - quaternion.w = (m12 - m21) * num4; - return quaternion; - } - if (m11 > m22) { - var num6 = (float)System.Math.Sqrt(((1f + m11) - m00) - m22); - var num3 = 0.5f / num6; - quaternion.x = (m10 + m01) * num3; - quaternion.y = 0.5f * num6; - quaternion.z = (m21 + m12) * num3; - quaternion.w = (m20 - m02) * num3; - return quaternion; - } - var num5 = (float)System.Math.Sqrt(((1f + m22) - m00) - m11); - var num2 = 0.5f / num5; - quaternion.x = (m20 + m02) * num2; - quaternion.y = (m21 + m12) * num2; - quaternion.z = 0.5f * num5; - quaternion.w = (m01 - m10) * num2; - return quaternion; - } - public void SetLookRotation(Vector3 view) { - Vector3 up = new Vector3(0, 0, 1); - this.SetLookRotation(view, up); - } - /// - /// Creates a rotation with the specified /forward/ and /upwards/ directions. - /// - /// The direction to look in. - /// The vector that defines in which direction up is. - public void SetLookRotation(Vector3 view, Vector3 up) { - this = MyQuaternion.LookRotation(view, up); - } - /// - /// Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is clamped to the range [0, 1]. - /// - /// - /// - /// - public static MyQuaternion Slerp(MyQuaternion a, MyQuaternion b, float t) { - return MyQuaternion.Slerp(ref a, ref b, t); - } - private static MyQuaternion Slerp(ref MyQuaternion a, ref MyQuaternion b, float t) { - if (t > 1) t = 1; - if (t < 0) t = 0; - return SlerpUnclamped(ref a, ref b, t); - } - /// - /// Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is not clamped. - /// - /// - /// - /// - public static MyQuaternion SlerpUnclamped(MyQuaternion a, MyQuaternion b, float t) { - return MyQuaternion.SlerpUnclamped(ref a, ref b, t); - } - private static MyQuaternion SlerpUnclamped(ref MyQuaternion a, ref MyQuaternion b, float t) { - // if either input is zero, return the other. - if (a.LengthSquared == 0.0f) { - if (b.LengthSquared == 0.0f) { - return identity; - } - return b; - } else if (b.LengthSquared == 0.0f) { - return a; - } - - - float cosHalfAngle = a.w * b.w + Vector3.Dot(a.xyz, b.xyz); - - if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { - // angle = 0.0f, so just return one input. - return a; - } else if (cosHalfAngle < 0.0f) { - b.xyz = -b.xyz; - b.w = -b.w; - cosHalfAngle = -cosHalfAngle; - } - - float blendA; - float blendB; - if (cosHalfAngle < 0.99f) { - // do proper slerp for big angles - float halfAngle = (float)System.Math.Acos(cosHalfAngle); - float sinHalfAngle = (float)System.Math.Sin(halfAngle); - float oneOverSinHalfAngle = 1.0f / sinHalfAngle; - blendA = (float)System.Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle; - blendB = (float)System.Math.Sin(halfAngle * t) * oneOverSinHalfAngle; - } else { - // do lerp if angle is really small. - blendA = 1.0f - t; - blendB = t; - } - - MyQuaternion result = new MyQuaternion(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w); - if (result.LengthSquared > 0.0f) - return Normalize(result); - else - return identity; - } - /// - /// Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is clamped to the range [0, 1]. - /// - /// - /// - /// - public static MyQuaternion Lerp(MyQuaternion a, MyQuaternion b, float t) { - if (t > 1) t = 1; - if (t < 0) t = 0; - return Slerp(ref a, ref b, t); // TODO: use lerp not slerp, "Because quaternion works in 4D. Rotation in 4D are linear" ??? - } - /// - /// Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is not clamped. - /// - /// - /// - /// - public static MyQuaternion LerpUnclamped(MyQuaternion a, MyQuaternion b, float t) { - return Slerp(ref a, ref b, t); - } - /// - /// Rotates a rotation /from/ towards /to/. - /// - /// - /// - /// - public static MyQuaternion RotateTowards(MyQuaternion from, MyQuaternion to, float maxDegreesDelta) { - float num = MyQuaternion.Angle(from, to); - if (num == 0f) { - return to; - } - float t = Math.Min(1f, maxDegreesDelta / num); - return MyQuaternion.SlerpUnclamped(from, to, t); - } - /// - /// Returns the Inverse of /rotation/. - /// - /// - public static MyQuaternion Inverse(MyQuaternion rotation) { - float lengthSq = rotation.LengthSquared; - if (lengthSq != 0.0) { - float i = 1.0f / lengthSq; - return new MyQuaternion(rotation.xyz * -i, rotation.w * i); - } - return rotation; - } - /// - /// Returns a nicely formatted string of the MyQuaternion. - /// - /// - public override string ToString() { - return string.Format("({0:F1}, {1:F1}, {2:F1}, {3:F1})", this.x, this.y, this.z, this.w); - } - /// - /// Returns a nicely formatted string of the MyQuaternion. - /// - /// - public string ToString(string format) { - return string.Format("({0}, {1}, {2}, {3})", this.x.ToString(format), this.y.ToString(format), this.z.ToString(format), this.w.ToString(format)); - } - /// - /// Returns the angle in degrees between two rotations /a/ and /b/. - /// - /// - /// - public static float Angle(MyQuaternion a, MyQuaternion b) { - float f = MyQuaternion.Dot(a, b); - return (float)Math.Acos(Math.Min( Math.Abs( f ), 1f) ) * 2f * radToDeg; - } - /// - /// Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order). - /// - /// - /// - /// - public static MyQuaternion Euler(float x, float y, float z) { - return MyQuaternion.FromEulerRad(new Vector3((float)x, (float)y, (float)z) * degToRad); - } - /// - /// Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order). - /// - /// - public static MyQuaternion Euler(Vector3 euler) { - return MyQuaternion.FromEulerRad(euler * degToRad); - } - // from http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine - private static Vector3 ToEulerRad(MyQuaternion rotation) { - float sqw = rotation.w * rotation.w; - float sqx = rotation.x * rotation.x; - float sqy = rotation.y * rotation.y; - float sqz = rotation.z * rotation.z; - float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor - float test = rotation.x * rotation.w - rotation.y * rotation.z; - Vector3 v; - - if (test > 0.4995f * unit) { // singularity at north pole - v.Y = 2.0f * (float)Math.Atan2(rotation.y, rotation.x); - v.X = (float) Math.PI / 2; - v.Z = 0; - return NormalizeAngles(v); - } - if (test < -0.4995f * unit) { // singularity at south pole - v.Y = -2f * (float) Math.Atan2(rotation.y, rotation.x); - v.X = -(float)Math.PI / 2; - v.Z = 0; - return NormalizeAngles(v); - } - MyQuaternion q = new MyQuaternion(rotation.w, rotation.z, rotation.x, rotation.y); - v.Y = (float)System.Math.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw - v.X = (float)System.Math.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch - v.Z = (float)System.Math.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll - return NormalizeAngles(v); - } - private static Vector3 NormalizeAngles(Vector3 angles) { - angles.X = NormalizeAngle(angles.X); - angles.Y = NormalizeAngle(angles.Y); - angles.Z = NormalizeAngle(angles.Z); - return angles; - } - private static float NormalizeAngle(float angle) { - while (angle > 360) - angle -= 360; - while (angle < 0) - angle += 360; - return angle; - } - // from http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha - private static MyQuaternion FromEulerRad(Vector3 euler) { - var yaw = euler.X; - var pitch = euler.Y; - var roll = euler.Z; - float rollOver2 = roll * 0.5f; - float sinRollOver2 = (float)System.Math.Sin((float)rollOver2); - float cosRollOver2 = (float)System.Math.Cos((float)rollOver2); - float pitchOver2 = pitch * 0.5f; - float sinPitchOver2 = (float)System.Math.Sin((float)pitchOver2); - float cosPitchOver2 = (float)System.Math.Cos((float)pitchOver2); - float yawOver2 = yaw * 0.5f; - float sinYawOver2 = (float)System.Math.Sin((float)yawOver2); - float cosYawOver2 = (float)System.Math.Cos((float)yawOver2); - MyQuaternion result; - result.x = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; - result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; - result.z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; - result.w = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; - return result; - - } - private static void ToAxisAngleRad(MyQuaternion q, out Vector3 axis, out float angle) { - if (System.Math.Abs(q.w) > 1.0f) - q.Normalize(); - angle = 2.0f * (float)System.Math.Acos(q.w); // angle - float den = (float)System.Math.Sqrt(1.0 - q.w * q.w); - if (den > 0.0001f) { - axis = q.xyz / den; - } else { - // This occurs when the angle is zero. - // Not a problem: just set an arbitrary normalized axis. - axis = new Vector3(1, 0, 0); - } - } - #region Obsolete methods - /* - [Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public static MyQuaternion EulerRotation(float x, float y, float z) - { - return MyQuaternion.Internal_FromEulerRad(new Vector3(x, y, z)); - } - [Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public static MyQuaternion EulerRotation(Vector3 euler) - { - return MyQuaternion.Internal_FromEulerRad(euler); - } - [Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public void SetEulerRotation(float x, float y, float z) - { - this = Quaternion.Internal_FromEulerRad(new Vector3(x, y, z)); - } - [Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public void SetEulerRotation(Vector3 euler) - { - this = Quaternion.Internal_FromEulerRad(euler); - } - [Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")] - public Vector3 ToEuler() - { - return Quaternion.Internal_ToEulerRad(this); - } - [Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public static Quaternion EulerAngles(float x, float y, float z) - { - return Quaternion.Internal_FromEulerRad(new Vector3(x, y, z)); - } - [Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public static Quaternion EulerAngles(Vector3 euler) - { - return Quaternion.Internal_FromEulerRad(euler); - } - [Obsolete("Use Quaternion.ToAngleAxis instead. This function was deprecated because it uses radians instead of degrees")] - public void ToAxisAngle(out Vector3 axis, out float angle) - { - Quaternion.Internal_ToAxisAngleRad(this, out axis, out angle); - } - [Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public void SetEulerAngles(float x, float y, float z) - { - this.SetEulerRotation(new Vector3(x, y, z)); - } - [Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")] - public void SetEulerAngles(Vector3 euler) - { - this = Quaternion.EulerRotation(euler); - } - [Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")] - public static Vector3 ToEulerAngles(Quaternion rotation) - { - return Quaternion.Internal_ToEulerRad(rotation); - } - [Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")] - public Vector3 ToEulerAngles() - { - return Quaternion.Internal_ToEulerRad(this); - } - [Obsolete("Use Quaternion.AngleAxis instead. This function was deprecated because it uses radians instead of degrees")] - public static Quaternion AxisAngle(Vector3 axis, float angle) - { - return Quaternion.INTERNAL_CALL_AxisAngle(ref axis, angle); - } - - private static Quaternion INTERNAL_CALL_AxisAngle(ref Vector3 axis, float angle) - { - - } - [Obsolete("Use Quaternion.AngleAxis instead. This function was deprecated because it uses radians instead of degrees")] - public void SetAxisAngle(Vector3 axis, float angle) - { - this = Quaternion.AxisAngle(axis, angle); - } - */ - #endregion - public override int GetHashCode() { - return this.x.GetHashCode() ^ this.y.GetHashCode() << 2 ^ this.z.GetHashCode() >> 2 ^ this.w.GetHashCode() >> 1; - } - public override bool Equals(object other) { - if (!(other is MyQuaternion)) { - return false; - } - MyQuaternion quaternion = (MyQuaternion)other; - return this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w); - } - public bool Equals(MyQuaternion other) { - return this.x.Equals(other.x) && this.y.Equals(other.y) && this.z.Equals(other.z) && this.w.Equals(other.w); - } - public static MyQuaternion operator *(MyQuaternion lhs, MyQuaternion rhs) { - return new MyQuaternion(lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y, lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z, lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x, lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z); - } - public static Vector3 operator *(MyQuaternion rotation, Vector3 point) { - float num = rotation.x * 2f; - float num2 = rotation.y * 2f; - float num3 = rotation.z * 2f; - float num4 = rotation.x * num; - float num5 = rotation.y * num2; - float num6 = rotation.z * num3; - float num7 = rotation.x * num2; - float num8 = rotation.x * num3; - float num9 = rotation.y * num3; - float num10 = rotation.w * num; - float num11 = rotation.w * num2; - float num12 = rotation.w * num3; - Vector3 result; - result.X = (1f - (num5 + num6)) * point.X + (num7 - num12) * point.Y + (num8 + num11) * point.Z; - result.Y = (num7 + num12) * point.X + (1f - (num4 + num6)) * point.Y + (num9 - num10) * point.Z; - result.Z = (num8 - num11) * point.X + (num9 + num10) * point.Y + (1f - (num4 + num5)) * point.Z; - return result; - } - public static bool operator ==(MyQuaternion lhs, MyQuaternion rhs) { - return MyQuaternion.Dot(lhs, rhs) > 0.999999f; - } - public static bool operator !=(MyQuaternion lhs, MyQuaternion rhs) { - return MyQuaternion.Dot(lhs, rhs) <= 0.999999f; - } -} \ No newline at end of file diff --git a/BetterJoyForCemu/Program.cs b/BetterJoyForCemu/Program.cs index de590f8..38daf15 100644 --- a/BetterJoyForCemu/Program.cs +++ b/BetterJoyForCemu/Program.cs @@ -18,6 +18,8 @@ namespace BetterJoyForCemu { public bool EnableIMU = true; public bool EnableLocalize = false; + + // Different operating systems either do or don't like the trailing zero private const ushort vendor_id = 0x57e; private const ushort vendor_id_ = 0x057e; @@ -72,7 +74,14 @@ namespace BetterJoyForCemu { IntPtr handle = HIDapi.hid_open_path(enumerate.path); HIDapi.hid_set_nonblocking(handle, 1); - j.Add(new Joycon(handle, EnableIMU, EnableLocalize & EnableIMU, 0.05f, isLeft, j.Count, enumerate.product_id == product_pro)); + + j.Add(new Joycon(handle, EnableIMU, EnableLocalize & EnableIMU, 0.05f, isLeft, j.Count, enumerate.product_id == product_pro, enumerate.serial_number == "000000000001")); + + byte[] mac = new byte[6]; + for (int n = 0; n < 6; n++) + mac[n] = byte.Parse(enumerate.serial_number.Substring(n * 2, 2), System.Globalization.NumberStyles.HexNumber); + j[j.Count - 1].PadMacAddress = new PhysicalAddress(mac); + ++i; } ptr = enumerate.next; @@ -150,10 +159,20 @@ namespace BetterJoyForCemu { } class Program { + public static PhysicalAddress btMAC = new PhysicalAddress(new byte[] { 0, 0, 0, 0, 0, 0 }); public static UdpServer server; static double pollsPerSecond = 120.0; static void Main(string[] args) { + foreach (NetworkInterface nic in NetworkInterface.GetAllNetworkInterfaces()) { + // Get local BT host MAC + if (nic.NetworkInterfaceType != NetworkInterfaceType.FastEthernetFx && nic.NetworkInterfaceType != NetworkInterfaceType.Wireless80211) { + if (nic.Name.Split()[0] == "Bluetooth") { + btMAC = nic.GetPhysicalAddress(); + } + } + } + JoyconManager mgr = new JoyconManager(); mgr.Awake(); mgr.Start();