DanceGame.cpp 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166
  1. #include "DanceGame.h"
  2. /*
  3. * 初始化DanceGame对象,分左右脚来判断
  4. */
  5. DanceGame::DanceGame()
  6. {
  7. leftFoot = DanceFoot(LEFT_FOOT);
  8. rightFoot = DanceFoot(RIGHT_FOOT);
  9. memset(result, -1, 4 * sizeof(int));
  10. left_result = -1;
  11. right_result = -1;
  12. }
  13. /*
  14. * DanceGame主接口,承接Game类的主要调用
  15. */
  16. void DanceGame::Process(int* right_pos, int* right_att, int right_zupt, int right_press,
  17. int* left_pos, int* left_att, int left_zupt, int left_press, int jump, int down, int rssi)
  18. {
  19. static int left_zupt_count = 0;
  20. static int right_zupt_count = 0;
  21. static float leftRotate[4] = { 1.0, 0.0, 0.0, 1.0 };
  22. static float rightRotate[4] = { 1.0, 0.0, 0.0, 1.0 };
  23. if (left_zupt && rssi < DANCEGAME_MIN_RSSI)
  24. {
  25. left_zupt_count++;
  26. }
  27. else
  28. {
  29. left_zupt_count = 0;
  30. }
  31. if (right_zupt && rssi < DANCEGAME_MIN_RSSI)
  32. {
  33. //rotateTrajRight.ResetHeading(right_att[0]);
  34. right_zupt_count++;
  35. }
  36. else
  37. {
  38. right_zupt_count = 0;
  39. }
  40. if (rssi > DANCEGAME_MIN_RSSI)
  41. {
  42. left_zupt_count = 0;
  43. right_zupt_count = 0;
  44. }
  45. if (left_zupt_count > 100 && right_zupt_count > 100)
  46. {
  47. float leftTheta = float(left_att[0]) * 0.0001f;
  48. float rightTheta = float(right_att[0]) * 0.0001f;
  49. leftRotate[0] = cos(leftTheta); leftRotate[1] = sin(leftTheta);
  50. leftRotate[2] = -sin(leftTheta); leftRotate[3] = cos(leftTheta);
  51. rightRotate[0] = cos(rightTheta); rightRotate[1] = sin(rightTheta);
  52. rightRotate[2] = -sin(rightTheta); rightRotate[3] = cos(rightTheta);
  53. std::cout << "matrix has inited!" << std::endl;
  54. left_zupt_count = 0;
  55. right_zupt_count = 0;
  56. }
  57. float left_pos_g[3];
  58. float pos_tmp[2];
  59. float right_pos_g[3];
  60. pos_tmp[0] = leftRotate[0] * left_pos[0] * 0.01 + leftRotate[1] * left_pos[1] * 0.01;
  61. pos_tmp[1] = leftRotate[2] * left_pos[0] * 0.01 + leftRotate[3] * left_pos[1] * 0.01;
  62. left_pos_g[0] = 0.9848 * pos_tmp[0] - 0.1736 * pos_tmp[1];
  63. left_pos_g[1] = 0.1736 * pos_tmp[0] + 0.9848 * pos_tmp[1];
  64. left_pos_g[2] = left_pos[2] * 0.01;
  65. pos_tmp[0] = rightRotate[0] * right_pos[0] * 0.01 + rightRotate[1] * right_pos[1] * 0.01f;
  66. pos_tmp[1] = rightRotate[2] * right_pos[0] * 0.01 + rightRotate[3] * right_pos[1] * 0.01f;
  67. right_pos_g[0] = 0.9848 * pos_tmp[0] + 0.1736 * pos_tmp[1];
  68. right_pos_g[1] = -0.1736 * pos_tmp[0] + 0.9848 * pos_tmp[1];
  69. right_pos_g[2] = right_pos[2] * 0.01;
  70. int left_result_tmp, right_result_tmp;
  71. left_result_tmp = leftFoot.calGlobalPos(left_pos_g, rssi, left_zupt, left_press);
  72. right_result_tmp = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt, right_press);
  73. if (left_result != left_result_tmp && left_result_tmp != -1)
  74. {
  75. result[0] = left_result_tmp;
  76. }
  77. else
  78. {
  79. result[0] = -1;
  80. }
  81. if(left_result != -1 && left_result_tmp == -1)
  82. {
  83. result[0] = left_result | MOTION_CANCEL;
  84. }
  85. left_result = left_result_tmp;
  86. if (right_result != right_result_tmp && right_result_tmp != -1)
  87. {
  88. result[1] = right_result_tmp;
  89. }
  90. else
  91. {
  92. result[1] = -1;
  93. }
  94. if (right_result != -1 && right_result_tmp == -1)
  95. {
  96. result[1] = right_result | MOTION_CANCEL;
  97. }
  98. right_result = right_result_tmp;
  99. /*
  100. result[0] = leftFoot.calGlobalPos(left_pos_g, rssi, left_zupt, left_press);
  101. result[1] = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt, right_press);
  102. */
  103. }
  104. /*
  105. * 获取跳舞游戏结果
  106. */
  107. void DanceGame::getResult(int *matrix)
  108. {
  109. memcpy(matrix, result, 4 * sizeof(int));
  110. }
  111. float DanceGame::getGamePos(int left_or_right, int index)
  112. {
  113. if (index < 0 || index > 2)
  114. return -1;
  115. if (left_or_right == LEFT_FOOT)
  116. return leftFoot.getGamePos(index);
  117. else
  118. return rightFoot.getGamePos(index);
  119. }