diff --git a/BetterJoyForCemu.sln b/BetterJoyForCemu.sln new file mode 100644 index 0000000..6a27a13 --- /dev/null +++ b/BetterJoyForCemu.sln @@ -0,0 +1,37 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.27130.2036 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BetterJoyForCemu", "BetterJoyForCemu\BetterJoyForCemu.csproj", "{1BF709E9-C133-41DF-933A-C9FF3F664C7B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x64.ActiveCfg = Debug|x64 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x64.Build.0 = Debug|x64 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x86.ActiveCfg = Debug|x86 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x86.Build.0 = Debug|x86 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|Any CPU.Build.0 = Release|Any CPU + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x64.ActiveCfg = Release|x64 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x64.Build.0 = Release|x64 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x86.ActiveCfg = Release|x86 + {1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {F928F866-3D27-4531-970D-25ADE4DDD979} + EndGlobalSection +EndGlobal diff --git a/BetterJoyForCemu/App.config b/BetterJoyForCemu/App.config new file mode 100644 index 0000000..731f6de --- /dev/null +++ b/BetterJoyForCemu/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/BetterJoyForCemu/BetterJoyForCemu.csproj b/BetterJoyForCemu/BetterJoyForCemu.csproj new file mode 100644 index 0000000..27c0098 --- /dev/null +++ b/BetterJoyForCemu/BetterJoyForCemu.csproj @@ -0,0 +1,106 @@ + + + + + Debug + AnyCPU + {1BF709E9-C133-41DF-933A-C9FF3F664C7B} + Exe + BetterJoyForCemu + BetterJoyForCemu + v4.6.1 + 512 + true + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + ..\packages\Crc32.NET.1.2.0\lib\net20\Crc32.NET.dll + + + + + + + + + + + + + + + + + + + + + + + + + + Always + + + + \ No newline at end of file diff --git a/BetterJoyForCemu/HIDapi.cs b/BetterJoyForCemu/HIDapi.cs new file mode 100644 index 0000000..7e7b4ad --- /dev/null +++ b/BetterJoyForCemu/HIDapi.cs @@ -0,0 +1,115 @@ +using System; +using System.Runtime.InteropServices; + +namespace BetterJoyForCemu { + public class HIDapi { + #if LINUX + const string dll = "libhidapi.so"; + #else + const string dll = "hidapi.dll"; + #endif + + public struct hid_device_info { + [MarshalAs(UnmanagedType.LPStr)] + public string path; + public ushort vendor_id; + public ushort product_id; + [MarshalAs(UnmanagedType.LPWStr)] + public string serial_number; + public ushort release_number; + [MarshalAs(UnmanagedType.LPWStr)] + public string manufacturer_string; + [MarshalAs(UnmanagedType.LPWStr)] + public string product_string; + public ushort usage_page; + public ushort usage; + public int interface_number; + public IntPtr next; + }; + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_init(); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_exit(); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr hid_enumerate(ushort vendor_id, ushort product_id); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern void hid_free_enumeration(IntPtr phid_device_info); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr hid_open(ushort vendor_id, ushort product_id, [MarshalAs(UnmanagedType.LPWStr)]string serial_number); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr hid_open_path([MarshalAs(UnmanagedType.LPStr)]string path); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_write(IntPtr device, byte[] data, UIntPtr length); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_read_timeout(IntPtr dev, byte[] data, UIntPtr length, int milliseconds); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_read(IntPtr device, byte[] data, UIntPtr length); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_set_nonblocking(IntPtr device, int nonblock); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_send_feature_report(IntPtr device, byte[] data, UIntPtr length); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_get_feature_report(IntPtr device, byte[] data, UIntPtr length); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern void hid_close(IntPtr device); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_get_manufacturer_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_get_product_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_get_serial_number_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + public static extern int hid_get_indexed_string(IntPtr device, int string_index, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen); + + [DllImport(dll, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.LPWStr)] + public static extern string hid_error(IntPtr device); + + static void PrintEnumeration(IntPtr phid_device_info) { + if (!phid_device_info.Equals(IntPtr.Zero)) { + hid_device_info hdev = (hid_device_info)Marshal.PtrToStructure(phid_device_info, typeof(hid_device_info)); + + Console.WriteLine(string.Format("path: {0}", hdev.path)); + Console.WriteLine(string.Format("vendor id: {0:X}", hdev.vendor_id)); + Console.WriteLine(string.Format("product id: {0:X}", hdev.product_id)); + Console.WriteLine(string.Format("usage page: {0:X}", hdev.usage_page)); + Console.WriteLine(string.Format("usage: {0:X}", hdev.usage)); + Console.WriteLine(""); + + PrintEnumeration(hdev.next); + } + } + + static string _getDevicePath(IntPtr phid_device_info, ushort usagePage, ushort usage) { + if (!phid_device_info.Equals(IntPtr.Zero)) { + hid_device_info hdev = (hid_device_info)Marshal.PtrToStructure(phid_device_info, typeof(hid_device_info)); + if (usagePage == hdev.usage_page && usage == hdev.usage) + return hdev.path; + else + return _getDevicePath(hdev.next, usagePage, usage); + } + return null; + } + + public static string GetDevicePath(ushort vendorId, ushort productId, ushort usagePage, ushort usage) { + return _getDevicePath(hid_enumerate(vendorId, productId), usagePage, usage); + } + } +} \ No newline at end of file diff --git a/BetterJoyForCemu/Joycon.cs b/BetterJoyForCemu/Joycon.cs new file mode 100644 index 0000000..de1cd63 --- /dev/null +++ b/BetterJoyForCemu/Joycon.cs @@ -0,0 +1,733 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Net.NetworkInformation; +using System.Numerics; +using System.Text; +using System.Threading; +using System.Threading.Tasks; + +namespace BetterJoyForCemu { + public class Joycon { + float timing = 60.0f; + + bool isPro = false; + + public enum DebugType : int { + NONE, + ALL, + COMMS, + THREADING, + IMU, + RUMBLE, + }; + public DebugType debug_type = DebugType.NONE; + public bool isLeft; + public enum state_ : uint { + NOT_ATTACHED, + DROPPED, + NO_JOYCONS, + ATTACHED, + INPUT_MODE_0x30, + IMU_DATA_OK, + }; + public state_ state; + public enum Button : int { + DPAD_DOWN = 0, + DPAD_RIGHT = 1, + DPAD_LEFT = 2, + DPAD_UP = 3, + SL = 4, + SR = 5, + MINUS = 6, + HOME = 7, + PLUS = 8, + CAPTURE = 9, + STICK = 10, + SHOULDER_1 = 11, + SHOULDER_2 = 12, + + // For pro controller + B = 13, + A = 14, + Y = 15, + X = 16, + STICK2 = 17, + SHOULDER2_1 = 18, + SHOULDER2_2 = 19, + }; + private bool[] buttons_down = new bool[20]; + private bool[] buttons_up = new bool[20]; + private bool[] buttons = new bool[20]; + private bool[] down_ = new bool[20]; + + private float[] stick = { 0, 0 }; + private float[] stick2 = { 0, 0 }; + + private + IntPtr handle; + + byte[] default_buf = { 0x0, 0x1, 0x40, 0x40, 0x0, 0x1, 0x40, 0x40 }; + + private byte[] stick_raw = { 0, 0, 0 }; + private UInt16[] stick_cal = { 0, 0, 0, 0, 0, 0 }; + private UInt16 deadzone; + private UInt16[] stick_precal = { 0, 0 }; + + private byte[] stick2_raw = { 0, 0, 0 }; + private UInt16[] stick2_cal = { 0, 0, 0, 0, 0, 0 }; + private UInt16 deadzone2; + private UInt16[] stick2_precal = { 0, 0 }; + + private bool stop_polling = false; + private int timestamp; + private bool first_imu_packet = true; + private bool imu_enabled = false; + private Int16[] acc_r = { 0, 0, 0 }; + private Int16[] acc_neutral = { 0, 0, 0 }; + private Int16[] acc_sensiti = { 0, 0, 0 }; + private Vector3 acc_g; + + private Int16[] gyr_r = { 0, 0, 0 }; + private Int16[] gyr_neutral = { 0, 0, 0 }; + private Int16[] gyr_sensiti = { 0, 0, 0 }; + private Vector3 gyr_g; + private bool do_localize; + private float filterweight; + private const uint report_len = 49; + private struct Report { + byte[] r; + System.DateTime t; + public ulong ts; + public Report(byte[] report, System.DateTime time, ulong timestamp) { + r = report; + t = time; + ts = timestamp; + } + public System.DateTime GetTime() { + return t; + } + public void CopyBuffer(byte[] b) { + for (int i = 0; i < report_len; ++i) { + b[i] = r[i]; + } + } + }; + private struct Rumble { + private float h_f, amp, l_f; + public float t; + public bool timed_rumble; + + public void set_vals(float low_freq, float high_freq, float amplitude, int time = 0) { + h_f = high_freq; + amp = amplitude; + l_f = low_freq; + timed_rumble = false; + t = 0; + if (time != 0) { + t = time / 1000f; + timed_rumble = true; + } + } + public Rumble(float low_freq, float high_freq, float amplitude, int time = 0) { + h_f = high_freq; + amp = amplitude; + l_f = low_freq; + timed_rumble = false; + t = 0; + if (time != 0) { + t = time / 1000f; + timed_rumble = true; + } + } + private float clamp(float x, float min, float max) { + if (x < min) return min; + if (x > max) return max; + return x; + } + public byte[] GetData() { + byte[] rumble_data = new byte[8]; + if (amp == 0.0f) { + rumble_data[0] = 0x0; + rumble_data[1] = 0x1; + rumble_data[2] = 0x40; + rumble_data[3] = 0x40; + } else { + l_f = clamp(l_f, 40.875885f, 626.286133f); + amp = clamp(amp, 0.0f, 1.0f); + h_f = clamp(h_f, 81.75177f, 1252.572266f); + UInt16 hf = (UInt16)((Math.Round(32f * Math.Log(h_f * 0.1f, 2)) - 0x60) * 4); + byte lf = (byte)(Math.Round(32f * Math.Log(l_f * 0.1f, 2)) - 0x40); + byte hf_amp; + if (amp == 0) hf_amp = 0; + else if (amp < 0.117) hf_amp = (byte)(((Math.Log(amp * 1000, 2) * 32) - 0x60) / (5 - Math.Pow(amp, 2)) - 1); + else if (amp < 0.23) hf_amp = (byte)(((Math.Log(amp * 1000, 2) * 32) - 0x60) - 0x5c); + else hf_amp = (byte)((((Math.Log(amp * 1000, 2) * 32) - 0x60) * 2) - 0xf6); + + UInt16 lf_amp = (UInt16)(Math.Round((double)hf_amp) * .5); + byte parity = (byte)(lf_amp % 2); + if (parity > 0) { + --lf_amp; + } + + lf_amp = (UInt16)(lf_amp >> 1); + lf_amp += 0x40; + if (parity > 0) lf_amp |= 0x8000; + rumble_data = new byte[8]; + rumble_data[0] = (byte)(hf & 0xff); + rumble_data[1] = (byte)((hf >> 8) & 0xff); + rumble_data[2] = lf; + rumble_data[1] += hf_amp; + rumble_data[2] += (byte)((lf_amp >> 8) & 0xff); + rumble_data[3] += (byte)(lf_amp & 0xff); + } + for (int i = 0; i < 4; ++i) { + rumble_data[4 + i] = rumble_data[i]; + } + //Debug.Log(string.Format("Encoded hex freq: {0:X2}", encoded_hex_freq)); + //Debug.Log(string.Format("lf_amp: {0:X4}", lf_amp)); + //Debug.Log(string.Format("hf_amp: {0:X2}", hf_amp)); + //Debug.Log(string.Format("l_f: {0:F}", l_f)); + //Debug.Log(string.Format("hf: {0:X4}", hf)); + //Debug.Log(string.Format("lf: {0:X2}", lf)); + return rumble_data; + } + } + private Queue reports = new Queue(); + private Rumble rumble_obj; + + private byte global_count = 0; + private string debug_str; + + // For UdpServer + public int PadId = 0; + public int battery = 2; + public int model = 2; + public int constate = 2; + public int connection = 3; + + public PhysicalAddress PadMacAddress = new PhysicalAddress(new byte[] { 01, 02, 03, 04, 05, 06 }); + public ulong Timestamp = (ulong)Stopwatch.GetTimestamp(); + public int packetCounter = 0; + // + + public Joycon(IntPtr handle_, bool imu, bool localize, float alpha, bool left, int id = 0, bool isPro=false) { + handle = handle_; + imu_enabled = imu; + do_localize = localize; + rumble_obj = new Rumble(160, 320, 0); + filterweight = alpha; + isLeft = left; + + PadId = id; + this.isPro = isPro; + } + public void DebugPrint(String s, DebugType d) { + if (debug_type == DebugType.NONE) return; + if (d == DebugType.ALL || d == debug_type || debug_type == DebugType.ALL) { + Console.WriteLine(s); + } + } + public bool GetButtonDown(Button b) { + return buttons_down[(int)b]; + } + public bool GetButton(Button b) { + return buttons[(int)b]; + } + public bool GetButtonUp(Button b) { + return buttons_up[(int)b]; + } + public float[] GetStick() { + return stick; + } + public float[] GetStick2() { + return stick2; + } + public Vector3 GetGyro() { + return gyr_g; + } + 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; + byte[] a = { 0x0 }; + // Input report mode + Subcommand(0x3, new byte[] { 0x3f }, 1, false); + 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); + 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); + DebugPrint("Done with init.", DebugType.COMMS); + return 0; + } + public void SetFilterCoeff(float a) { + filterweight = a; + } + public void Detach() { + stop_polling = true; + PrintArray(max, format: "Max {0:S}", d: DebugType.IMU); + PrintArray(sum, format: "Sum {0:S}", d: DebugType.IMU); + if (state > state_.NO_JOYCONS) { + Subcommand(0x30, new byte[] { 0x0 }, 1); + Subcommand(0x40, new byte[] { 0x0 }, 1); + Subcommand(0x48, new byte[] { 0x0 }, 1); + Subcommand(0x3, new byte[] { 0x3f }, 1); + } + if (state > state_.DROPPED) { + HIDapi.hid_close(handle); + } + state = state_.NOT_ATTACHED; + } + private byte ts_en; + private byte ts_de; + private System.DateTime ts_prev; + private int ReceiveRaw() { + if (handle == IntPtr.Zero) return -2; + HIDapi.hid_set_nonblocking(handle, 0); + byte[] raw_buf = new byte[report_len]; + int ret = HIDapi.hid_read(handle, raw_buf, new UIntPtr(report_len)); + if (ret > 0) { + lock (reports) { + reports.Enqueue(new Report(raw_buf, System.DateTime.Now, (ulong)Stopwatch.GetTimestamp())); + } + if (ts_en == raw_buf[1]) { + DebugPrint(string.Format("Duplicate timestamp enqueued. TS: {0:X2}", ts_en), DebugType.THREADING); + } + ts_en = raw_buf[1]; + DebugPrint(string.Format("Enqueue. Bytes read: {0:D}. Timestamp: {1:X2}", ret, raw_buf[1]), DebugType.THREADING); + } + return ret; + } + private Thread PollThreadObj; + private void Poll() { + int attempts = 0; + while (!stop_polling & state > state_.NO_JOYCONS) { + SendRumble(rumble_obj.GetData()); + int a = ReceiveRaw(); + //a = ReceiveRaw(); + if (a > 0) { + state = state_.IMU_DATA_OK; + attempts = 0; + } else if (attempts > 1000) { + state = state_.DROPPED; + DebugPrint("Connection lost. Is the Joy-Con connected?", DebugType.ALL); + break; + } else { + DebugPrint("Pause 5ms", DebugType.THREADING); + Thread.Sleep((Int32)5); + } + ++attempts; + } + DebugPrint("End poll loop.", DebugType.THREADING); + } + float[] max = { 0, 0, 0 }; + float[] sum = { 0, 0, 0 }; + public void Update() { + if (state > state_.NO_JOYCONS) { + byte[] report_buf = new byte[report_len]; + while (reports.Count > 0) { + Report rep; + lock (reports) { + rep = reports.Dequeue(); + rep.CopyBuffer(report_buf); + } + if (imu_enabled) { + if (do_localize) { + ProcessIMU(report_buf); + } else { + ExtractIMUValues(report_buf, 0); + } + } + if (ts_de == report_buf[1]) { + DebugPrint(string.Format("Duplicate timestamp dequeued. TS: {0:X2}", ts_de), DebugType.THREADING); + } + ts_de = report_buf[1]; + DebugPrint(String.Format("Dequeue. Queue length: {0}. Packet ID: {1}. Timestamp: {2}. Lag to dequeue: {3}. Lag between packets (expect 15ms): {4}", reports.Count, report_buf[0], report_buf[1], System.DateTime.Now.Subtract(rep.GetTime()), rep.GetTime().Subtract(ts_prev)), DebugType.THREADING); + ts_prev = rep.GetTime(); + + // set timestamp and packet count for server + Timestamp = rep.ts; + } + ProcessButtonsAndStick(report_buf); + if (rumble_obj.timed_rumble) { + if (rumble_obj.t < 0) { + rumble_obj.set_vals(160, 320, 0, 0); + } else { + rumble_obj.t -= (1 / timing); + } + } + + packetCounter++; + Program.server.NewReportIncoming(this); + } + } + private int ProcessButtonsAndStick(byte[] report_buf) { + if (report_buf[0] == 0x00) return -1; + + stick_raw[0] = report_buf[6 + (isLeft ? 0 : 3)]; + stick_raw[1] = report_buf[7 + (isLeft ? 0 : 3)]; + stick_raw[2] = report_buf[8 + (isLeft ? 0 : 3)]; + + if (isPro) { + stick2_raw[0] = report_buf[6 + (!isLeft ? 0 : 3)]; + stick2_raw[1] = report_buf[7 + (!isLeft ? 0 : 3)]; + stick2_raw[2] = report_buf[8 + (!isLeft ? 0 : 3)]; + } + + stick_precal[0] = (UInt16)(stick_raw[0] | ((stick_raw[1] & 0xf) << 8)); + stick_precal[1] = (UInt16)((stick_raw[1] >> 4) | (stick_raw[2] << 4)); + stick = CenterSticks(stick_precal); + + if (isPro) { + stick2_precal[0] = (UInt16)(stick2_raw[0] | ((stick2_raw[1] & 0xf) << 8)); + stick2_precal[1] = (UInt16)((stick2_raw[1] >> 4) | (stick2_raw[2] << 4)); + stick2 = CenterSticks(stick2_precal, true); + } + + lock (buttons) { + lock (down_) { + for (int i = 0; i < buttons.Length; ++i) { + down_[i] = buttons[i]; + } + } + buttons[(int)Button.DPAD_DOWN] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x01 : 0x04)) != 0; + buttons[(int)Button.DPAD_RIGHT] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x04 : 0x08)) != 0; + buttons[(int)Button.DPAD_UP] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x02 : 0x02)) != 0; + buttons[(int)Button.DPAD_LEFT] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x08 : 0x01)) != 0; + buttons[(int)Button.HOME] = ((report_buf[4] & 0x10) != 0); + buttons[(int)Button.MINUS] = ((report_buf[4] & 0x01) != 0); + buttons[(int)Button.PLUS] = ((report_buf[4] & 0x02) != 0); + buttons[(int)Button.STICK] = ((report_buf[4] & (isLeft ? 0x08 : 0x04)) != 0); + buttons[(int)Button.SHOULDER_1] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x40) != 0; + buttons[(int)Button.SHOULDER_2] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x80) != 0; + buttons[(int)Button.SR] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x10) != 0; + buttons[(int)Button.SL] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x20) != 0; + + if (isPro) { + buttons[(int)Button.B] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x01 : 0x04)) != 0; + buttons[(int)Button.A] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x04 : 0x08)) != 0; + buttons[(int)Button.X] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x02 : 0x02)) != 0; + buttons[(int)Button.Y] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x08 : 0x01)) != 0; + + buttons[(int)Button.STICK2] = ((report_buf[4] & (!isLeft ? 0x08 : 0x04)) != 0); + buttons[(int)Button.SHOULDER2_1] = (report_buf[3 + (!isLeft ? 2 : 0)] & 0x40) != 0; + buttons[(int)Button.SHOULDER2_2] = (report_buf[3 + (!isLeft ? 2 : 0)] & 0x80) != 0; + } + + lock (buttons_up) { + lock (buttons_down) { + for (int i = 0; i < buttons.Length; ++i) { + buttons_up[i] = (down_[i] & !buttons[i]); + buttons_down[i] = (!down_[i] & buttons[i]); + } + } + } + } + return 0; + } + private void ExtractIMUValues(byte[] report_buf, int n = 0) { + gyr_r[0] = (Int16)(report_buf[19 + n * 12] | ((report_buf[20 + n * 12] << 8) & 0xff00)); + gyr_r[1] = (Int16)(report_buf[21 + n * 12] | ((report_buf[22 + n * 12] << 8) & 0xff00)); + gyr_r[2] = (Int16)(report_buf[23 + n * 12] | ((report_buf[24 + n * 12] << 8) & 0xff00)); + acc_r[0] = (Int16)(report_buf[13 + n * 12] | ((report_buf[14 + n * 12] << 8) & 0xff00)); + acc_r[1] = (Int16)(report_buf[15 + n * 12] | ((report_buf[16 + n * 12] << 8) & 0xff00)); + acc_r[2] = (Int16)(report_buf[17 + n * 12] | ((report_buf[18 + n * 12] << 8) & 0xff00)); + for (int i = 0; i < 3; ++i) { + switch (i) { + case 0: + acc_g.X = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; + //gyr_g.X = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f; + gyr_g.X = gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f; // Neutrals may be read wrong + if (Math.Abs(acc_g.X) > Math.Abs(max[i])) + max[i] = acc_g.X; + break; + case 1: + acc_g.Y = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; + //gyr_g.Y = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f; + gyr_g.Y = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f; + if (Math.Abs(acc_g.Y) > Math.Abs(max[i])) + max[i] = acc_g.Y; + break; + case 2: + acc_g.Z = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f; + //gyr_g.Z = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f; + gyr_g.Z = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f; + if (Math.Abs(acc_g.Z) > Math.Abs(max[i])) + max[i] = acc_g.Z; + 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)); + PollThreadObj.Start(); + + Console.WriteLine("Starting poll thread."); + } + } + public void Recenter() { + first_imu_packet = true; + } + private float[] CenterSticks(UInt16[] vals, bool special=false) { + ushort[] t = stick_cal; + + if (special) + t = stick2_cal; + + float[] s = { 0, 0 }; + for (uint i = 0; i < 2; ++i) { + float diff = vals[i] - t[2 + i]; + if (Math.Abs(diff) < deadzone) vals[i] = 0; + else if (diff > 0) // if axis is above center + { + s[i] = diff / t[i]; + } else { + s[i] = diff / t[4 + i]; + } + } + 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; + buf_[1] = global_count; + if (global_count == 0xf) global_count = 0; + else ++global_count; + Array.Copy(buf, 0, buf_, 2, 8); + 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]; + Array.Copy(default_buf, 0, buf_, 2, 8); + Array.Copy(buf, 0, buf_, 11, len); + buf_[10] = sc; + buf_[1] = global_count; + buf_[0] = 0x1; + if (global_count == 0xf) global_count = 0; + else ++global_count; + if (print) { PrintArray(buf_, DebugType.COMMS, len, 11, "Subcommand 0x" + string.Format("{0:X2}", sc) + " sent. Data: 0x{0:S}"); }; + HIDapi.hid_write(handle, buf_, new UIntPtr(len + 11)); + int res = HIDapi.hid_read_timeout(handle, response, new UIntPtr(report_len), 50); + if (res < 1) DebugPrint("No response.", DebugType.COMMS); + 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; + for (int i = 0; i < 9; ++i) { + if (buf_[i] != 0xff) { + Console.WriteLine("Using user stick calibration data."); + found = true; + break; + } + } + if (!found) { + Console.WriteLine("Using factory stick calibration data."); + buf_ = ReadSPI(0x60, (isLeft ? (byte)0x3d : (byte)0x46), 9); // get user calibration data if possible + } + stick_cal[isLeft ? 0 : 2] = (UInt16)((buf_[1] << 8) & 0xF00 | buf_[0]); // X Axis Max above center + stick_cal[isLeft ? 1 : 3] = (UInt16)((buf_[2] << 4) | (buf_[1] >> 4)); // Y Axis Max above center + stick_cal[isLeft ? 2 : 4] = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); // X Axis Center + stick_cal[isLeft ? 3 : 5] = (UInt16)((buf_[5] << 4) | (buf_[4] >> 4)); // Y Axis Center + stick_cal[isLeft ? 4 : 0] = (UInt16)((buf_[7] << 8) & 0xF00 | buf_[6]); // X Axis Min below center + stick_cal[isLeft ? 5 : 1] = (UInt16)((buf_[8] << 4) | (buf_[7] >> 4)); // Y Axis Min below center + + PrintArray(stick_cal, len: 6, start: 0, format: "Stick calibration data: {0:S}"); + + if (isPro) { + buf_ = ReadSPI(0x80, (!isLeft ? (byte)0x12 : (byte)0x1d), 9); // get user calibration data if possible + found = false; + for (int i = 0; i < 9; ++i) { + if (buf_[i] != 0xff) { + Console.WriteLine("Using user stick calibration data."); + found = true; + break; + } + } + if (!found) { + Console.WriteLine("Using factory stick calibration data."); + buf_ = ReadSPI(0x60, (!isLeft ? (byte)0x3d : (byte)0x46), 9); // get user calibration data if possible + } + stick2_cal[!isLeft ? 0 : 2] = (UInt16)((buf_[1] << 8) & 0xF00 | buf_[0]); // X Axis Max above center + stick2_cal[!isLeft ? 1 : 3] = (UInt16)((buf_[2] << 4) | (buf_[1] >> 4)); // Y Axis Max above center + stick2_cal[!isLeft ? 2 : 4] = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); // X Axis Center + stick2_cal[!isLeft ? 3 : 5] = (UInt16)((buf_[5] << 4) | (buf_[4] >> 4)); // Y Axis Center + stick2_cal[!isLeft ? 4 : 0] = (UInt16)((buf_[7] << 8) & 0xF00 | buf_[6]); // X Axis Min below center + stick2_cal[!isLeft ? 5 : 1] = (UInt16)((buf_[8] << 4) | (buf_[7] >> 4)); // Y Axis Min below center + + PrintArray(stick2_cal, len: 6, start: 0, format: "Stick calibration data: {0:S}"); + + buf_ = ReadSPI(0x60, (!isLeft ? (byte)0x86 : (byte)0x98), 16); + deadzone2 = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); + } + + buf_ = ReadSPI(0x60, (isLeft ? (byte)0x86 : (byte)0x98), 16); + deadzone = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); + + buf_ = ReadSPI(0x80, 0x28, 10); + acc_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + acc_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + acc_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x80, 0x2E, 10); + acc_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + acc_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + acc_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x80, 0x34, 10); + gyr_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + gyr_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + gyr_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x80, 0x3A, 10); + gyr_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + gyr_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + gyr_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + PrintArray(gyr_neutral, len: 3, d: DebugType.IMU, format: "User gyro neutral position: {0:S}"); + + // This is an extremely messy way of checking to see whether there is user stick calibration data present, but I've seen conflicting user calibration data on blank Joy-Cons. Worth another look eventually. + if (gyr_neutral[0] + gyr_neutral[1] + gyr_neutral[2] == -3 || Math.Abs(gyr_neutral[0]) > 100 || Math.Abs(gyr_neutral[1]) > 100 || Math.Abs(gyr_neutral[2]) > 100) { + buf_ = ReadSPI(0x60, 0x20, 10); + acc_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + acc_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + acc_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x60, 0x26, 10); + acc_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + acc_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + acc_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x60, 0x2C, 10); + gyr_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + gyr_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + gyr_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + buf_ = ReadSPI(0x60, 0x32, 10); + gyr_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00)); + gyr_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00)); + gyr_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00)); + + 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]; + byte[] buf_ = new byte[len + 20]; + + for (int i = 0; i < 100; ++i) { + buf_ = Subcommand(0x10, buf, 5, false); + if (buf_[15] == addr2 && buf_[16] == addr1) { + break; + } + } + Array.Copy(buf_, 20, read_buf, 0, len); + 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; + string tostr = ""; + for (int i = 0; i < len; ++i) { + tostr += string.Format((arr[0] is byte) ? "{0:X2} " : ((arr[0] is float) ? "{0:F} " : "{0:D} "), arr[i + start]); + } + DebugPrint(string.Format(format, tostr), d); + } + } +} diff --git a/BetterJoyForCemu/MyQuaternion.cs b/BetterJoyForCemu/MyQuaternion.cs new file mode 100644 index 0000000..ce6956a --- /dev/null +++ b/BetterJoyForCemu/MyQuaternion.cs @@ -0,0 +1,675 @@ +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 new file mode 100644 index 0000000..70365f6 --- /dev/null +++ b/BetterJoyForCemu/Program.cs @@ -0,0 +1,151 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +using System.Numerics; +using System.Threading; +using System.Runtime.InteropServices; +using System.Timers; +using static BetterJoyForCemu.HIDapi; +using System.Net.NetworkInformation; +using System.Diagnostics; + +namespace BetterJoyForCemu { + public class JoyconManager { + // Settings accessible via Unity + public bool EnableIMU = true; + public bool EnableLocalize = true; + + // Different operating systems either do or don't like the trailing zero + private const ushort vendor_id = 0x57e; + private const ushort vendor_id_ = 0x057e; + private const ushort product_l = 0x2006; + private const ushort product_r = 0x2007; + private const ushort product_pro = 0x2009; + + public List j; // Array of all connected Joy-Cons + static JoyconManager instance; + + public static JoyconManager Instance { + get { return instance; } + } + + public void Awake() { + instance = this; + int i = 0; + + j = new List(); + bool isLeft = false; + HIDapi.hid_init(); + + IntPtr ptr = HIDapi.hid_enumerate(vendor_id, 0x0); + IntPtr top_ptr = ptr; + + if (ptr == IntPtr.Zero) { + ptr = HIDapi.hid_enumerate(vendor_id_, 0x0); + if (ptr == IntPtr.Zero) { + HIDapi.hid_free_enumeration(ptr); + Console.WriteLine("No Joy-Cons found!"); + } + } + + string path = ""; + hid_device_info enumerate; + while (ptr != IntPtr.Zero) { + enumerate = (hid_device_info)Marshal.PtrToStructure(ptr, typeof(hid_device_info)); + + if (enumerate.product_id == product_l || enumerate.product_id == product_r || enumerate.product_id == product_pro) { + if (enumerate.product_id == product_l) { + isLeft = true; + Console.WriteLine("Left Joy-Con connected."); + } else if (enumerate.product_id == product_r) { + isLeft = false; + Console.WriteLine("Right Joy-Con connected."); + } else if (enumerate.product_id == product_pro) { + isLeft = true; + Console.WriteLine("Pro controller connected."); + } else { + Console.WriteLine("Non Joy-Con input device skipped."); + } + + 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)); + ++i; + } + ptr = enumerate.next; + } + + HIDapi.hid_free_enumeration(top_ptr); + } + + public void Start() { + for (int i = 0; i < j.Count; ++i) { + Joycon jc = j[i]; + byte LEDs = 0x0; + LEDs |= (byte)(0x1 << i); + jc.Attach(leds_: LEDs); + jc.Begin(); + } + } + + public bool shouldUpdate = true; + public void Update(object sender, ElapsedEventArgs e) { + //while (shouldUpdate) { + for (int i = 0; i < j.Count; ++i) { + j[i].Update(); + + /*if (j.Count > 0) { + Joycon jj = j[i]; + + if (jj.GetButtonDown(Joycon.Button.DPAD_DOWN)) + jj.SetRumble(160, 320, 0.6f, 200); + }*/ + } + //} + } + + public void OnApplicationQuit() { + for (int i = 0; i < j.Count; ++i) { + j[i].Detach(); + } + } + } + + class Program { + public static UdpServer server; + static float pollsPerSecond = 60.0f; + + static void Main(string[] args) { + JoyconManager mgr = new JoyconManager(); + mgr.Awake(); + mgr.Start(); + + server = new UdpServer(mgr.j); + + //updateThread = new Thread(new ThreadStart(mgr.Update)); + //updateThread.Start(); + + server.Start(26760); + System.Timers.Timer timer = new System.Timers.Timer((int)(1000 / pollsPerSecond)); + timer.Elapsed += mgr.Update; + timer.Elapsed += printt; + timer.Start(); + + Console.Write("Press enter to quit."); + Console.ReadLine(); + + server.Stop(); + mgr.shouldUpdate = false; + timer.Stop(); + timer.Dispose(); + mgr.OnApplicationQuit(); + } + + static void printt(object sender, ElapsedEventArgs e) { + //Console.Write('.'); + } + } +} diff --git a/BetterJoyForCemu/Properties/AssemblyInfo.cs b/BetterJoyForCemu/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..1f8f1ca --- /dev/null +++ b/BetterJoyForCemu/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("BetterJoyForCemu")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("BetterJoyForCemu")] +[assembly: AssemblyCopyright("Copyright © 2018")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("1bf709e9-c133-41df-933a-c9ff3f664c7b")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/BetterJoyForCemu/UpdServer.cs b/BetterJoyForCemu/UpdServer.cs new file mode 100644 index 0000000..940ced5 --- /dev/null +++ b/BetterJoyForCemu/UpdServer.cs @@ -0,0 +1,515 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +using Force.Crc32; +using System.Net; +using System.Net.NetworkInformation; +using System.Net.Sockets; + +namespace BetterJoyForCemu { + class UdpServer { + private Socket udpSock; + private uint serverId; + private bool running; + private byte[] recvBuffer = new byte[1024]; + + List controllers; + + public UdpServer(List p) { + controllers = p; + } + + enum MessageType { + DSUC_VersionReq = 0x100000, + DSUS_VersionRsp = 0x100000, + DSUC_ListPorts = 0x100001, + DSUS_PortInfo = 0x100001, + DSUC_PadDataReq = 0x100002, + DSUS_PadDataRsp = 0x100002, + }; + + private const ushort MaxProtocolVersion = 1001; + + class ClientRequestTimes { + DateTime allPads; + DateTime[] padIds; + Dictionary padMacs; + + public DateTime AllPadsTime { get { return allPads; } } + public DateTime[] PadIdsTime { get { return padIds; } } + public Dictionary PadMacsTime { get { return padMacs; } } + + public ClientRequestTimes() { + allPads = DateTime.MinValue; + padIds = new DateTime[4]; + + for (int i = 0; i < padIds.Length; i++) + padIds[i] = DateTime.MinValue; + + padMacs = new Dictionary(); + } + + public void RequestPadInfo(byte regFlags, byte idToReg, PhysicalAddress macToReg) { + if (regFlags == 0) + allPads = DateTime.UtcNow; + else { + if ((regFlags & 0x01) != 0) //id valid + { + if (idToReg < padIds.Length) + padIds[idToReg] = DateTime.UtcNow; + } + if ((regFlags & 0x02) != 0) //mac valid + { + padMacs[macToReg] = DateTime.UtcNow; + } + } + } + } + + private Dictionary clients = new Dictionary(); + + private int BeginPacket(byte[] packetBuf, ushort reqProtocolVersion = MaxProtocolVersion) { + int currIdx = 0; + packetBuf[currIdx++] = (byte)'D'; + packetBuf[currIdx++] = (byte)'S'; + packetBuf[currIdx++] = (byte)'U'; + packetBuf[currIdx++] = (byte)'S'; + + Array.Copy(BitConverter.GetBytes((ushort)reqProtocolVersion), 0, packetBuf, currIdx, 2); + currIdx += 2; + + Array.Copy(BitConverter.GetBytes((ushort)packetBuf.Length - 16), 0, packetBuf, currIdx, 2); + currIdx += 2; + + Array.Clear(packetBuf, currIdx, 4); //place for crc + currIdx += 4; + + Array.Copy(BitConverter.GetBytes((uint)serverId), 0, packetBuf, currIdx, 4); + currIdx += 4; + + return currIdx; + } + + private void FinishPacket(byte[] packetBuf) { + Array.Clear(packetBuf, 8, 4); + + uint crcCalc = Crc32Algorithm.Compute(packetBuf); + Array.Copy(BitConverter.GetBytes((uint)crcCalc), 0, packetBuf, 8, 4); + } + + private void SendPacket(IPEndPoint clientEP, byte[] usefulData, ushort reqProtocolVersion = MaxProtocolVersion) { + byte[] packetData = new byte[usefulData.Length + 16]; + int currIdx = BeginPacket(packetData, reqProtocolVersion); + Array.Copy(usefulData, 0, packetData, currIdx, usefulData.Length); + FinishPacket(packetData); + + try { udpSock.SendTo(packetData, clientEP); } catch (Exception e) { } + } + + private void ProcessIncoming(byte[] localMsg, IPEndPoint clientEP) { + try { + int currIdx = 0; + if (localMsg[0] != 'D' || localMsg[1] != 'S' || localMsg[2] != 'U' || localMsg[3] != 'C') + return; + else + currIdx += 4; + + uint protocolVer = BitConverter.ToUInt16(localMsg, currIdx); + currIdx += 2; + + if (protocolVer > MaxProtocolVersion) + return; + + uint packetSize = BitConverter.ToUInt16(localMsg, currIdx); + currIdx += 2; + + if (packetSize < 0) + return; + + packetSize += 16; //size of header + if (packetSize > localMsg.Length) + return; + else if (packetSize < localMsg.Length) { + byte[] newMsg = new byte[packetSize]; + Array.Copy(localMsg, newMsg, packetSize); + localMsg = newMsg; + } + + uint crcValue = BitConverter.ToUInt32(localMsg, currIdx); + //zero out the crc32 in the packet once we got it since that's whats needed for calculation + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + + uint crcCalc = Crc32Algorithm.Compute(localMsg); + if (crcValue != crcCalc) + return; + + uint clientId = BitConverter.ToUInt32(localMsg, currIdx); + currIdx += 4; + + uint messageType = BitConverter.ToUInt32(localMsg, currIdx); + currIdx += 4; + + if (messageType == (uint)MessageType.DSUC_VersionReq) { + byte[] outputData = new byte[8]; + int outIdx = 0; + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_VersionRsp), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((ushort)MaxProtocolVersion), 0, outputData, outIdx, 2); + outIdx += 2; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + + SendPacket(clientEP, outputData, 1001); + } else if (messageType == (uint)MessageType.DSUC_ListPorts) { + // Requested information on gamepads - return MAC address + int numPadRequests = BitConverter.ToInt32(localMsg, currIdx); + currIdx += 4; + if (numPadRequests < 0 || numPadRequests > 4) + return; + + int requestsIdx = currIdx; + for (int i = 0; i < numPadRequests; i++) { + byte currRequest = localMsg[requestsIdx + i]; + if (currRequest < 0 || currRequest > 4) + return; + } + + byte[] outputData = new byte[16]; + for (byte i = 0; i < numPadRequests; i++) { + byte currRequest = localMsg[requestsIdx + i]; + var padData = controllers[i];//controllers[currRequest]; + + int outIdx = 0; + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PortInfo), 0, outputData, outIdx, 4); + outIdx += 4; + + outputData[outIdx++] = (byte)padData.PadId; + outputData[outIdx++] = (byte)padData.constate; + outputData[outIdx++] = (byte)padData.model; + outputData[outIdx++] = (byte)padData.connection; + + var addressBytes = padData.PadMacAddress.GetAddressBytes(); + if (addressBytes.Length == 6) { + outputData[outIdx++] = addressBytes[0]; + outputData[outIdx++] = addressBytes[1]; + outputData[outIdx++] = addressBytes[2]; + outputData[outIdx++] = addressBytes[3]; + outputData[outIdx++] = addressBytes[4]; + outputData[outIdx++] = addressBytes[5]; + } else { + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + } + + outputData[outIdx++] = (byte)padData.battery;//(byte)padData.BatteryStatus; + outputData[outIdx++] = 0; + + SendPacket(clientEP, outputData, 1001); + } + } else if (messageType == (uint)MessageType.DSUC_PadDataReq) { + byte regFlags = localMsg[currIdx++]; + byte idToReg = localMsg[currIdx++]; + PhysicalAddress macToReg = null; + { + byte[] macBytes = new byte[6]; + Array.Copy(localMsg, currIdx, macBytes, 0, macBytes.Length); + currIdx += macBytes.Length; + macToReg = new PhysicalAddress(macBytes); + } + + lock (clients) { + if (clients.ContainsKey(clientEP)) + clients[clientEP].RequestPadInfo(regFlags, idToReg, macToReg); + else { + var clientTimes = new ClientRequestTimes(); + clientTimes.RequestPadInfo(regFlags, idToReg, macToReg); + clients[clientEP] = clientTimes; + } + } + } + } catch (Exception e) { } + } + + private void ReceiveCallback(IAsyncResult iar) { + byte[] localMsg = null; + EndPoint clientEP = new IPEndPoint(IPAddress.Any, 0); + + try { + //Get the received message. + Socket recvSock = (Socket)iar.AsyncState; + int msgLen = recvSock.EndReceiveFrom(iar, ref clientEP); + + localMsg = new byte[msgLen]; + Array.Copy(recvBuffer, localMsg, msgLen); + } catch (Exception e) { } + + //Start another receive as soon as we copied the data + StartReceive(); + + //Process the data if its valid + if (localMsg != null) { + ProcessIncoming(localMsg, (IPEndPoint)clientEP); + } + } + private void StartReceive() { + try { + if (running) { + //Start listening for a new message. + EndPoint newClientEP = new IPEndPoint(IPAddress.Any, 0); + udpSock.BeginReceiveFrom(recvBuffer, 0, recvBuffer.Length, SocketFlags.None, ref newClientEP, ReceiveCallback, udpSock); + } + } catch (SocketException ex) { + uint IOC_IN = 0x80000000; + uint IOC_VENDOR = 0x18000000; + uint SIO_UDP_CONNRESET = IOC_IN | IOC_VENDOR | 12; + udpSock.IOControl((int)SIO_UDP_CONNRESET, new byte[] { Convert.ToByte(false) }, null); + + StartReceive(); + } + } + + public void Start(int port=26760) { + if (running) { + if (udpSock != null) { + udpSock.Close(); + udpSock = null; + } + running = false; + } + + udpSock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp); + try { udpSock.Bind(new IPEndPoint(IPAddress.Loopback, port)); } catch (SocketException ex) { + udpSock.Close(); + udpSock = null; + + throw ex; + } + + byte[] randomBuf = new byte[4]; + new Random().NextBytes(randomBuf); + serverId = BitConverter.ToUInt32(randomBuf, 0); + + running = true; + Console.WriteLine("Starting server."); + StartReceive(); + } + + public void Stop() { + running = false; + if (udpSock != null) { + udpSock.Close(); + udpSock = null; + } + } + + private bool ReportToBuffer(Joycon hidReport, byte[] outputData, ref int outIdx) { + outputData[outIdx] = 0; + + if (hidReport.GetButton(Joycon.Button.DPAD_LEFT)) outputData[outIdx] |= 0x80; + if (hidReport.GetButton(Joycon.Button.DPAD_DOWN)) outputData[outIdx] |= 0x40; + if (hidReport.GetButton(Joycon.Button.DPAD_RIGHT)) outputData[outIdx] |= 0x20; + if (hidReport.GetButton(Joycon.Button.DPAD_UP)) outputData[outIdx] |= 0x10; + + if (hidReport.GetButton(Joycon.Button.PLUS)) outputData[outIdx] |= 0x08; + if (hidReport.GetButton(Joycon.Button.STICK2)) outputData[outIdx] |= 0x04; + if (hidReport.GetButton(Joycon.Button.STICK)) outputData[outIdx] |= 0x02; + if (hidReport.GetButton(Joycon.Button.MINUS)) outputData[outIdx] |= 0x01; + + outputData[++outIdx] = 0; + + if (hidReport.GetButton(Joycon.Button.Y)) outputData[outIdx] |= 0x80; + if (hidReport.GetButton(Joycon.Button.B)) outputData[outIdx] |= 0x40; + if (hidReport.GetButton(Joycon.Button.A)) outputData[outIdx] |= 0x20; + if (hidReport.GetButton(Joycon.Button.X)) outputData[outIdx] |= 0x10; + + if (hidReport.GetButton(Joycon.Button.SHOULDER2_1)) outputData[outIdx] |= 0x08; + if (hidReport.GetButton(Joycon.Button.SHOULDER_1)) outputData[outIdx] |= 0x04; + if (hidReport.GetButton(Joycon.Button.SHOULDER2_2)) outputData[outIdx] |= 0x02; + if (hidReport.GetButton(Joycon.Button.SHOULDER_2)) outputData[outIdx] |= 0x01; + + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.HOME)) ? (byte)1 : (byte)0; + outputData[++outIdx] = 0; // no touch pad + + float[] leftStick = hidReport.GetStick(); // 127 is 0 + outputData[++outIdx] = (byte) (Math.Max(0, Math.Min(255, 127 + leftStick[0] * 127))); + outputData[++outIdx] = (byte) (Math.Max(0, Math.Min(255, 127 + leftStick[1] * 127))); + + float[] rightStick = hidReport.GetStick2(); // 127 is 0 + outputData[++outIdx] = (byte)(Math.Max(0, Math.Min(255, 127 + rightStick[0] * 127))); + outputData[++outIdx] = (byte)(Math.Max(0, Math.Min(255, 127 + rightStick[1] * 127))); + + //we don't have analog buttons so just use the Button enums (which give either 0 or 0xFF) + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_LEFT)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_DOWN)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_RIGHT)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_UP)) ? (byte)0xFF : (byte)0; + + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.Y)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.B)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.A)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.X)) ? (byte)0xFF : (byte)0; + + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER2_1)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER_1)) ? (byte)0xFF : (byte)0; + + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER2_2)) ? (byte)0xFF : (byte)0; + outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER_2)) ? (byte)0xFF : (byte)0; + + outIdx++; + + //DS4 only: touchpad points + for (int i = 0; i < 2; i++) { + outIdx += 6; + } + + //motion timestamp + Array.Copy(BitConverter.GetBytes(hidReport.Timestamp), 0, outputData, outIdx, 8); + outIdx += 8; + + //accelerometer + { + var accel = hidReport.GetAccel(); + if (accel != null) { + Array.Copy(BitConverter.GetBytes(accel.Y), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes(-accel.Z), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes(accel.X), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + } else { + outIdx += 12; + Console.WriteLine("No accelerometer reported."); + } + } + + //gyroscope + { + var gyro = hidReport.GetGyro(); + if (gyro != null) { + Array.Copy(BitConverter.GetBytes(gyro.Y), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes(gyro.Z), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes(gyro.X), 0, outputData, outIdx, 4); + //Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4); + outIdx += 4; + } else { + outIdx += 12; + Console.WriteLine("No gyroscope reported."); + } + } + + return true; + } + + public void NewReportIncoming(Joycon hidReport) { + if (!running) + return; + + //Console.WriteLine("New report!"); + + var clientsList = new List(); + var now = DateTime.UtcNow; + lock (clients) { + var clientsToDelete = new List(); + + foreach (var cl in clients) { + const double TimeoutLimit = 5; + + if ((now - cl.Value.AllPadsTime).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else if ((hidReport.PadId >= 0 && hidReport.PadId <= 3) && + (now - cl.Value.PadIdsTime[(byte)hidReport.PadId]).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else if (cl.Value.PadMacsTime.ContainsKey(hidReport.PadMacAddress) && + (now - cl.Value.PadMacsTime[hidReport.PadMacAddress]).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else //check if this client is totally dead, and remove it if so + { + bool clientOk = false; + for (int i = 0; i < cl.Value.PadIdsTime.Length; i++) { + var dur = (now - cl.Value.PadIdsTime[i]).TotalSeconds; + if (dur < TimeoutLimit) { + clientOk = true; + break; + } + } + if (!clientOk) { + foreach (var dict in cl.Value.PadMacsTime) { + var dur = (now - dict.Value).TotalSeconds; + if (dur < TimeoutLimit) { + clientOk = true; + break; + } + } + + if (!clientOk) + clientsToDelete.Add(cl.Key); + } + } + } + + foreach (var delCl in clientsToDelete) { + clients.Remove(delCl); + } + clientsToDelete.Clear(); + clientsToDelete = null; + } + + if (clientsList.Count <= 0) + return; + + byte[] outputData = new byte[100]; + int outIdx = BeginPacket(outputData, 1001); + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PadDataRsp), 0, outputData, outIdx, 4); + outIdx += 4; + + outputData[outIdx++] = (byte)hidReport.PadId; //(byte)hidReport.PadId; + outputData[outIdx++] = (byte)hidReport.constate; //(byte)hidReport.PadState; + outputData[outIdx++] = (byte)hidReport.model; //(byte)hidReport.Model; + outputData[outIdx++] = (byte)hidReport.connection; //(byte)hidReport.ConnectionType; + { + byte[] padMac = hidReport.PadMacAddress.GetAddressBytes(); + outputData[outIdx++] = padMac[0]; + outputData[outIdx++] = padMac[1]; + outputData[outIdx++] = padMac[2]; + outputData[outIdx++] = padMac[3]; + outputData[outIdx++] = padMac[4]; + outputData[outIdx++] = padMac[5]; + } + + outputData[outIdx++] = (byte)hidReport.battery;//(byte)hidReport.BatteryStatus; + outputData[outIdx++] = 1;// hidReport.IsPadActive ? (byte)1 : (byte)0; + + // (uint)hidReport.PacketCounter + Array.Copy(BitConverter.GetBytes(hidReport.packetCounter), 0, outputData, outIdx, 4); + outIdx += 4; + + if (!ReportToBuffer(hidReport, outputData, ref outIdx)) + return; + else + FinishPacket(outputData); + + foreach (var cl in clientsList) { + try { udpSock.SendTo(outputData, cl); } catch (SocketException ex) { } + } + clientsList.Clear(); + clientsList = null; + } + } +} diff --git a/BetterJoyForCemu/hidapi.dll b/BetterJoyForCemu/hidapi.dll new file mode 100644 index 0000000..bfb6c27 Binary files /dev/null and b/BetterJoyForCemu/hidapi.dll differ diff --git a/BetterJoyForCemu/packages.config b/BetterJoyForCemu/packages.config new file mode 100644 index 0000000..f6de8b7 --- /dev/null +++ b/BetterJoyForCemu/packages.config @@ -0,0 +1,4 @@ + + + + \ No newline at end of file