Advertisement
Guest User

Untitled

a guest
Feb 8th, 2018
306
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 11.69 KB | None | 0 0
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement