SHARE
TWEET

Untitled

a guest Feb 8th, 2018 97 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "stdafx.h"
  2.  
  3. #include <stdint.h>
  4. #include <xtgmath.h>
  5. #include "include\sixense_math.hpp"
  6.  
  7. #include <iostream>
  8. #include <fstream>
  9. #include <vector>
  10. #include <array>
  11. #include <algorithm>
  12.  
  13. struct emulated_data
  14. {
  15.     float yaw, pitch, roll;
  16.     float x, y, z;
  17.     float joystick_x;
  18.     float joystick_y;
  19.     float trigger;
  20.     unsigned int buttons;
  21.     int enabled;
  22.     int controller_index;
  23.     unsigned char is_docked;
  24.     unsigned char which_hand;
  25. };
  26.  
  27. #define SIXENSE_BUTTON_BUMPER   (0x01<<7)
  28. #define SIXENSE_BUTTON_JOYSTICK (0x01<<8)
  29. #define SIXENSE_BUTTON_1        (0x01<<5)
  30. #define SIXENSE_BUTTON_2        (0x01<<6)
  31. #define SIXENSE_BUTTON_3        (0x01<<3)
  32. #define SIXENSE_BUTTON_4        (0x01<<4)
  33. #define SIXENSE_BUTTON_START    (0x01<<0)
  34.  
  35. #define SIXENSE_SUCCESS 0
  36. #define SIXENSE_FAILURE -1
  37.  
  38. #define SIXENSE_MAX_CONTROLLERS 4
  39.  
  40. #define SIXENSE_EXPORT extern "C" __declspec(dllexport)
  41.  
  42. typedef struct _sixenseControllerData {
  43.     float pos[3];
  44.     float rot_mat[3][3];
  45.     float joystick_x;
  46.     float joystick_y;
  47.     float trigger;
  48.     unsigned int buttons;
  49.     unsigned char sequence_number;
  50.     float rot_quat[4];
  51.     unsigned short firmware_revision;
  52.     unsigned short hardware_revision;
  53.     unsigned short packet_type;
  54.     unsigned short magnetic_frequency;
  55.     int enabled;
  56.     int controller_index;
  57.     unsigned char is_docked;
  58.     unsigned char which_hand;
  59.     unsigned char hemi_tracking_enabled;
  60. } sixenseControllerData;
  61.  
  62. typedef struct _sixenseAllControllerData {
  63.     sixenseControllerData controllers[4];
  64. } sixenseAllControllerData;
  65.  
  66.  
  67. SIXENSE_EXPORT int sixenseInit(void);
  68. SIXENSE_EXPORT int sixenseExit(void);
  69.  
  70. SIXENSE_EXPORT int sixenseGetMaxBases();
  71. SIXENSE_EXPORT int sixenseSetActiveBase(int i);
  72. SIXENSE_EXPORT int sixenseIsBaseConnected(int i);
  73.  
  74. SIXENSE_EXPORT int sixenseGetMaxControllers(void);
  75. SIXENSE_EXPORT int sixenseIsControllerEnabled(int which);
  76. SIXENSE_EXPORT int sixenseGetNumActiveControllers();
  77.  
  78. SIXENSE_EXPORT int sixenseGetHistorySize();
  79.  
  80. SIXENSE_EXPORT int sixenseGetData(int which, int index_back, sixenseControllerData *);
  81. SIXENSE_EXPORT int sixenseGetAllData(int index_back, sixenseAllControllerData *);
  82. SIXENSE_EXPORT int sixenseGetNewestData(int which, sixenseControllerData *);
  83. SIXENSE_EXPORT int sixenseGetAllNewestData(sixenseAllControllerData *);
  84.  
  85. SIXENSE_EXPORT int sixenseSetHemisphereTrackingMode(int which_controller, int state);
  86. SIXENSE_EXPORT int sixenseGetHemisphereTrackingMode(int which_controller, int *state);
  87.  
  88. SIXENSE_EXPORT int sixenseAutoEnableHemisphereTracking(int which_controller);
  89.  
  90. SIXENSE_EXPORT int sixenseSetHighPriorityBindingEnabled(int on_or_off);
  91. SIXENSE_EXPORT int sixenseGetHighPriorityBindingEnabled(int *on_or_off);
  92.  
  93. SIXENSE_EXPORT int sixenseTriggerVibration(int controller_id, int duration_100ms, int pattern_id);
  94.  
  95. SIXENSE_EXPORT int sixenseSetFilterEnabled(int on_or_off);
  96. SIXENSE_EXPORT int sixenseGetFilterEnabled(int *on_or_off);
  97.  
  98. SIXENSE_EXPORT int sixenseSetFilterParams(float near_range, float near_val, float far_range, float far_val);
  99. SIXENSE_EXPORT int sixenseGetFilterParams(float *near_range, float *near_val, float *far_range, float *far_val);
  100.  
  101. SIXENSE_EXPORT int sixenseSetBaseColor(unsigned char red, unsigned char green, unsigned char blue);
  102. SIXENSE_EXPORT int sixenseGetBaseColor(unsigned char *red, unsigned char *green, unsigned char *blue);
  103.  
  104. BOOL APIENTRY DllMain(HMODULE hModule, DWORD  ul_reason_for_call, LPVOID lpReserved)
  105. {
  106.     /*switch (ul_reason_for_call)
  107.     {
  108.     case DLL_PROCESS_ATTACH:
  109.     //case DLL_THREAD_ATTACH:
  110.     //case DLL_THREAD_DETACH:
  111.     case DLL_PROCESS_DETACH:
  112.     break;
  113.     }*/
  114.     return TRUE;
  115. }
  116.  
  117. SIXENSE_EXPORT int sixenseInit(void)
  118. {
  119.     return SIXENSE_SUCCESS;
  120. }
  121.  
  122. SIXENSE_EXPORT int sixenseExit(void)
  123. {
  124.     return SIXENSE_SUCCESS;
  125. }
  126.  
  127. SIXENSE_EXPORT int sixenseGetMaxBases()
  128. {
  129.     return 4;
  130. }
  131. SIXENSE_EXPORT int sixenseSetActiveBase(int i)
  132. {
  133.     return SIXENSE_SUCCESS;
  134. }
  135. SIXENSE_EXPORT int sixenseIsBaseConnected(int i)
  136. {
  137.     if (i == 0)
  138.         return 1;
  139.     return 0;
  140. }
  141.  
  142. SIXENSE_EXPORT int sixenseGetMaxControllers(void)
  143. {
  144.     return 4;
  145. }
  146.  
  147. SIXENSE_EXPORT int sixenseIsControllerEnabled(int which)
  148. {
  149.     if (which <= 1)
  150.         return 1;
  151.     return 0;
  152. }
  153.  
  154. SIXENSE_EXPORT int sixenseGetNumActiveControllers()
  155. {
  156.     return 2;
  157. }
  158.  
  159. SIXENSE_EXPORT int sixenseGetHistorySize()
  160. {
  161.     return 0;
  162. }
  163.  
  164. void convert_euler(float yaw, float pitch, float roll, sixenseControllerData *output) {
  165.     auto mat =
  166.         sixenseMath::Matrix3::rotation(roll, sixenseMath::Vector3(0, 0, -1)) *
  167.         sixenseMath::Matrix3::rotation(pitch, sixenseMath::Vector3(1, 0, 0)) *
  168.         sixenseMath::Matrix3::rotation(yaw, sixenseMath::Vector3(0, -1, 0));
  169.     auto quat = sixenseMath::Quat(mat);
  170.  
  171.     quat.fill((float*)&output->rot_quat);
  172.     mat.fill((float(*)[3])&output->rot_mat);
  173. }
  174.  
  175. std::array<uint8_t, 2> sequence_numbers = { 0 };
  176.  
  177. double myPosX1 = 100, myPosY1 = 0, myPosZ1 = 0, yaw1 = 0, roll1 = 0, pitch1 = 0;
  178. double myPosX2 = -100, myPosY2 = 0, myPosZ2 = 0, yaw2 = 0, roll2 = 0, pitch2 = 0;
  179.  
  180. double DegToRad(double f) {
  181.     return f * (3.14159265358979323846 / 180);
  182. }
  183.  
  184. SIXENSE_EXPORT int sixenseGetData(int which, int index_back, sixenseControllerData *output)
  185. {
  186.     if (index_back != 0 || which > 2)
  187.         return SIXENSE_FAILURE;
  188.  
  189.     //auto view = shared_memory.open_view();
  190.  
  191.     //auto data = view.map()[which];
  192.  
  193.     /*output->sequence_number = sequence_numbers[which]++;
  194.  
  195.     output->buttons = data.buttons;
  196.     output->trigger = data.trigger;
  197.     output->controller_index = which;
  198.     output->which_hand = data.which_hand;
  199.     output->enabled = data.enabled;
  200.     output->firmware_revision = 174;
  201.     output->hardware_revision = 0;
  202.     output->is_docked = data.is_docked;
  203.     output->joystick_x = data.joystick_x;
  204.     output->joystick_y = data.joystick_y;
  205.     output->pos[0] = data.x;
  206.     output->pos[1] = data.y;
  207.     output->pos[2] = data.z;
  208.     output->hemi_tracking_enabled = 1;
  209.     output->magnetic_frequency = 0;
  210.     output->packet_type = 1;
  211.  
  212.     convert_euler(data.yaw, data.pitch, data.roll, output); */
  213.  
  214.     output->buttons = 0;
  215.     output->trigger = 0;
  216.     output->controller_index = which;
  217.     output->which_hand = 0;
  218.  
  219.     output->enabled = true;
  220.     output->firmware_revision = 174;
  221.     output->hardware_revision = 0;
  222.     output->is_docked = 0;
  223.     output->joystick_x = 0;
  224.     output->joystick_y = 0;
  225.     output->pos[0] = 0;
  226.     output->pos[1] = 0;
  227.     output->pos[2] = 0;
  228.     output->hemi_tracking_enabled = 1;
  229.     output->magnetic_frequency = 0;
  230.     output->packet_type = 1;
  231.  
  232.     if ((GetAsyncKeyState(0x52) & 0x8000) != 0) { //Сброс всего
  233.         myPosX1 = 100;
  234.         myPosY1 = 0;
  235.         myPosZ1 = 0;
  236.         yaw1 = 0;
  237.         pitch1 = 0;
  238.         roll1 = 0;
  239.  
  240.         myPosX2 = -100;
  241.         myPosY2 = 0;
  242.         myPosZ2 = 0;
  243.         yaw2 = 0;
  244.         pitch2 = 0;
  245.         roll2 = 0;
  246.     }
  247.  
  248.     if (which == 0) {
  249.         //X
  250.         if ((GetAsyncKeyState(0x57) & 0x8000) != 0) myPosY1 += 1; //W
  251.         if ((GetAsyncKeyState(0x53) & 0x8000) != 0) myPosY1 -= 1; //S
  252.         //Y
  253.         if ((GetAsyncKeyState(0x41) & 0x8000) != 0) myPosX1 += 1; //A
  254.         if ((GetAsyncKeyState(0x44) & 0x8000) != 0) myPosX1 -= 1; //D
  255.         //Z
  256.         if ((GetAsyncKeyState(0x51) & 0x8000) != 0) myPosZ1 += 1; //Q
  257.         if ((GetAsyncKeyState(0x45) & 0x8000) != 0) myPosZ1 -= 1; //E
  258.  
  259.         if ((GetAsyncKeyState(0x55) & 0x8000) != 0) yaw1 += 1; //U
  260.         if (yaw1 > 179) yaw1 = -179;
  261.  
  262.         if ((GetAsyncKeyState(0x48) & 0x8000) != 0) roll1 += 1; //H
  263.         if (roll1 > 179) roll1 = -179;
  264.  
  265.         if ((GetAsyncKeyState(0x4B) & 0x8000) != 0) pitch1 += 1; //K
  266.         if (pitch1 > 179) pitch1 = -179;
  267.  
  268.         output->which_hand = 0;
  269.         output->pos[0] = myPosX1;
  270.         output->pos[1] = myPosZ1;
  271.         output->pos[2] = myPosY1;
  272.  
  273.         if ((GetAsyncKeyState(0x31) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_BUMPER; //1
  274.         if ((GetAsyncKeyState(0x32) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_JOYSTICK; //2
  275.         if ((GetAsyncKeyState(0x33) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_1; //3
  276.         if ((GetAsyncKeyState(0x34) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_2; //4
  277.         if ((GetAsyncKeyState(0x35) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_3; //5
  278.         if ((GetAsyncKeyState(0x36) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_4; //6
  279.         if ((GetAsyncKeyState(0x37) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_START; //7
  280.         if ((GetAsyncKeyState(0x38) & 0x8000) != 0) output->trigger = 1; //8
  281.  
  282.         convert_euler(DegToRad(yaw1), DegToRad(pitch1), DegToRad(roll1), output);
  283.     }
  284.     else {
  285.         if ((GetAsyncKeyState(VK_UP) & 0x8000) != 0) myPosY2 += 1;
  286.         if ((GetAsyncKeyState(VK_DOWN) & 0x8000) != 0) myPosY2 -= 1;
  287.         if ((GetAsyncKeyState(VK_LEFT) & 0x8000) != 0) myPosX2 += 1;
  288.         if ((GetAsyncKeyState(VK_RIGHT) & 0x8000) != 0) myPosX2 -= 1;
  289.         if ((GetAsyncKeyState(VK_PRIOR) & 0x8000) != 0) myPosZ2 += 1; //PageUP
  290.         if ((GetAsyncKeyState(VK_NEXT) & 0x8000) != 0) myPosZ2 -= 1; //PageDown
  291.  
  292.         if ((GetAsyncKeyState(VK_INSERT) & 0x8000) != 0) yaw2 += 1;
  293.         if (yaw2 > 179) yaw2 = -179;
  294.  
  295.         if ((GetAsyncKeyState(VK_DELETE) & 0x8000) != 0) roll2 += 1; //
  296.         if (roll2 > 179) roll2 = -179;
  297.  
  298.         if ((GetAsyncKeyState(VK_END) & 0x8000) != 0) pitch2 += 1; //
  299.         if (pitch2 > 179) pitch2 = -179;
  300.  
  301.         output->which_hand = 1;
  302.         output->pos[0] = myPosX2;
  303.         output->pos[1] = myPosZ2;
  304.         output->pos[2] = myPosY2;
  305.  
  306.         if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_BUMPER; //
  307.         if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_JOYSTICK; //
  308.         if ((GetAsyncKeyState(VK_NUMPAD3) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_1; //
  309.         if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_2; //
  310.         if ((GetAsyncKeyState(VK_NUMPAD5) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_3; //
  311.         if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_4; //
  312.         if ((GetAsyncKeyState(VK_NUMPAD7) & 0x8000) != 0) output->buttons = SIXENSE_BUTTON_START; //
  313.         if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) output->trigger = 1; //
  314.  
  315.         convert_euler(DegToRad(yaw2), DegToRad(pitch2), DegToRad(roll2), output);
  316.     }
  317.  
  318.     return SIXENSE_SUCCESS;
  319. }
  320.  
  321. SIXENSE_EXPORT int sixenseGetAllData(int index_back, sixenseAllControllerData *output)
  322. {
  323.     auto success = sixenseGetData(0, index_back, &output->controllers[0]);
  324.     success |= sixenseGetData(1, index_back, &output->controllers[1]);
  325.     return success;
  326. }
  327. SIXENSE_EXPORT int sixenseGetNewestData(int which, sixenseControllerData *output)
  328. {
  329.     return sixenseGetData(which, 0, output);
  330. }
  331.  
  332. SIXENSE_EXPORT int sixenseGetAllNewestData(sixenseAllControllerData *output)
  333. {
  334.     return sixenseGetAllData(0, output);
  335. }
  336.  
  337. SIXENSE_EXPORT int sixenseSetHemisphereTrackingMode(int which_controller, int state)
  338. {
  339.     return SIXENSE_SUCCESS;
  340. }
  341. SIXENSE_EXPORT int sixenseGetHemisphereTrackingMode(int which_controller, int *state)
  342. {
  343.     return SIXENSE_SUCCESS;
  344. }
  345.  
  346. SIXENSE_EXPORT int sixenseAutoEnableHemisphereTracking(int which_controller)
  347. {
  348.     return SIXENSE_SUCCESS;
  349. }
  350.  
  351. SIXENSE_EXPORT int sixenseSetHighPriorityBindingEnabled(int on_or_off)
  352. {
  353.     return SIXENSE_SUCCESS;
  354. }
  355. SIXENSE_EXPORT int sixenseGetHighPriorityBindingEnabled(int *on_or_off)
  356. {
  357.     return SIXENSE_SUCCESS;
  358. }
  359.  
  360. SIXENSE_EXPORT int sixenseTriggerVibration(int controller_id, int duration_100ms, int pattern_id)
  361. {
  362.     return SIXENSE_SUCCESS;
  363. }
  364.  
  365. SIXENSE_EXPORT int sixenseSetFilterEnabled(int on_or_off)
  366. {
  367.     return SIXENSE_SUCCESS;
  368. }
  369. SIXENSE_EXPORT int sixenseGetFilterEnabled(int *on_or_off)
  370. {
  371.     return SIXENSE_SUCCESS;
  372. }
  373.  
  374. SIXENSE_EXPORT int sixenseSetFilterParams(float near_range, float near_val, float far_range, float far_val)
  375. {
  376.     return SIXENSE_SUCCESS;
  377. }
  378.  
  379. SIXENSE_EXPORT int sixenseGetFilterParams(float *near_range, float *near_val, float *far_range, float *far_val)
  380. {
  381.     return SIXENSE_SUCCESS;
  382. }
  383.  
  384. SIXENSE_EXPORT int sixenseSetBaseColor(unsigned char red, unsigned char green, unsigned char blue)
  385. {
  386.     return SIXENSE_SUCCESS;
  387. }
  388. SIXENSE_EXPORT int sixenseGetBaseColor(unsigned char *red, unsigned char *green, unsigned char *blue)
  389. {
  390.     return SIXENSE_SUCCESS;
  391. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top