BodySourceView.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388
  1. using System.Collections.Generic;
  2. using UnityEngine;
  3. using Kinect = Windows.Kinect;
  4. using Valve.VR;
  5. public class BodySourceView : MonoBehaviour
  6. {
  7. public Transform[] joints;
  8. public Material BoneMaterial;
  9. public GameObject BodySourceManager;
  10. public Transform cameraRig;
  11. public PlayerReplay playerReplay;
  12. public ModeController modeController;
  13. public bool wristLeftLate;
  14. public bool wristRightLate;
  15. private Dictionary<ulong, GameObject> _Bodies = new Dictionary<ulong, GameObject>();
  16. private BodySourceManager _BodyManager;
  17. private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
  18. {
  19. { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
  20. { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
  21. { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
  22. { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },
  23. { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
  24. { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
  25. { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
  26. { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },
  27. { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
  28. { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
  29. { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
  30. { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
  31. { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
  32. { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },
  33. { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
  34. { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
  35. { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
  36. { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
  37. { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
  38. { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },
  39. { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
  40. { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
  41. { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
  42. { Kinect.JointType.Neck, Kinect.JointType.Head },
  43. };
  44. void Update()
  45. {
  46. if (BodySourceManager == null)
  47. {
  48. return;
  49. }
  50. _BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
  51. if (_BodyManager == null)
  52. {
  53. return;
  54. }
  55. Kinect.Body[] data = _BodyManager.GetData();
  56. if (data == null)
  57. {
  58. return;
  59. }
  60. List<ulong> trackedIds = new List<ulong>();
  61. foreach (var body in data)
  62. {
  63. if (body == null)
  64. {
  65. continue;
  66. }
  67. if (body.IsTracked)
  68. {
  69. trackedIds.Add(body.TrackingId);
  70. }
  71. }
  72. List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
  73. // First delete untracked bodies
  74. foreach (ulong trackingId in knownIds)
  75. {
  76. if (!trackedIds.Contains(trackingId))
  77. {
  78. Destroy(_Bodies[trackingId]);
  79. _Bodies.Remove(trackingId);
  80. }
  81. }
  82. foreach (var body in data)
  83. {
  84. if (body == null)
  85. {
  86. continue;
  87. }
  88. if (body.IsTracked)
  89. {
  90. if (!_Bodies.ContainsKey(body.TrackingId))
  91. {
  92. _Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
  93. }
  94. RefreshBodyObject(body, _Bodies[body.TrackingId]);
  95. }
  96. }
  97. }
  98. private GameObject CreateBodyObject(ulong id)
  99. {
  100. GameObject body = new GameObject("Body:" + id);
  101. body.transform.parent = gameObject.transform;
  102. for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
  103. {
  104. GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
  105. // Give WristLeft and WristRight rigidbody
  106. if (jt == Kinect.JointType.WristLeft)
  107. {
  108. jointObj.tag = "WristLeft";
  109. Rigidbody rb = jointObj.AddComponent<Rigidbody>();
  110. rb.useGravity = false;
  111. rb.isKinematic = true;
  112. }
  113. else if (jt == Kinect.JointType.WristRight)
  114. {
  115. jointObj.tag = "WristRight";
  116. Rigidbody rb = jointObj.AddComponent<Rigidbody>();
  117. rb.useGravity = false;
  118. rb.isKinematic = true;
  119. }
  120. LineRenderer lr = jointObj.AddComponent<LineRenderer>();
  121. //lr.SetVertexCount(2);
  122. lr.positionCount = 2;
  123. //lr.material = BoneMaterial;
  124. //lr.SetWidth(0.05f, 0.05f);
  125. lr.startWidth = 0.05f;
  126. lr.endWidth = 0.05f;
  127. Material whiteDiffuseMat = new Material(Shader.Find("Sprites/Default"));
  128. lr.material = whiteDiffuseMat;
  129. jointObj.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
  130. jointObj.name = jt.ToString();
  131. jointObj.transform.parent = body.transform;
  132. }
  133. return body;
  134. }
  135. private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
  136. {
  137. Vector3[] initJoints = new Vector3[25];
  138. for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
  139. {
  140. Kinect.Joint sourceJoint = body.Joints[jt];
  141. initJoints[(int)jt] = GetVector3FromJoint(sourceJoint);
  142. }
  143. // TODO: use this method
  144. Vector3[] rescaledJoints = RescaleJoints(initJoints);
  145. for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
  146. {
  147. Kinect.Joint sourceJoint = body.Joints[jt];
  148. Kinect.Joint? targetJoint = null;
  149. if (_BoneMap.ContainsKey(jt))
  150. {
  151. targetJoint = body.Joints[_BoneMap[jt]];
  152. }
  153. Transform jointObj = bodyObject.transform.Find(jt.ToString());
  154. //jointObj.localPosition = GetVector3FromJoint(sourceJoint);
  155. jointObj.localPosition = rescaledJoints[(int)jt];
  156. if (jt == Kinect.JointType.Head)
  157. {
  158. // Make head joint invisible
  159. jointObj.GetComponent<Renderer>().enabled = false;
  160. }
  161. if (jt == Kinect.JointType.Neck)
  162. {
  163. // Make camera follow the neck
  164. if (modeController.perspective == ModeController.Perspective.FirstPersonPerspective)
  165. {
  166. cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y + 0.08f, jointObj.localPosition.z - 0.03f);
  167. }
  168. else
  169. {
  170. cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y - 0.1f, jointObj.localPosition.z + 1);
  171. }
  172. }
  173. // Make these joints invisible
  174. if (jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight
  175. || jt == Kinect.JointType.HandLeft || jt == Kinect.JointType.HandRight
  176. || jt == Kinect.JointType.HandTipLeft || jt == Kinect.JointType.HandTipRight)
  177. {
  178. jointObj.GetComponent<Renderer>().enabled = false;
  179. }
  180. // WristLeft and WristRight do something if not following the motions
  181. if ((jt == Kinect.JointType.WristLeft && wristLeftLate) || (jt == Kinect.JointType.WristRight && wristRightLate))
  182. {
  183. if (modeController.feedback == ModeController.Feedback.ColorFeedback)
  184. {
  185. jointObj.GetComponent<Renderer>().material.color = new Color(1, 0, 0);
  186. jointObj.GetComponent<LineRenderer>().startColor = new Color(1, 0, 0);
  187. }
  188. else if (modeController.feedback == ModeController.Feedback.HapticFeedback)
  189. {
  190. // TODO: Add haptic feedback need to test
  191. if (jt == Kinect.JointType.WristLeft)
  192. {
  193. Debug.Log("WristLeft vibrate");
  194. SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftHand].Execute(0, 0.01f, 10, 1);
  195. }
  196. else if (jt == Kinect.JointType.WristRight)
  197. {
  198. Debug.Log("WristRight vibrate");
  199. SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightHand].Execute(0, 0.01f, 10, 1);
  200. }
  201. }
  202. }
  203. LineRenderer lr = jointObj.GetComponent<LineRenderer>();
  204. if (targetJoint.HasValue && jt != Kinect.JointType.Neck && jt != Kinect.JointType.ThumbLeft && jt != Kinect.JointType.ThumbRight
  205. && jt != Kinect.JointType.HandLeft && jt != Kinect.JointType.HandRight
  206. && jt != Kinect.JointType.HandTipLeft && jt != Kinect.JointType.HandTipRight) // Do not make LineRenderer for these joints
  207. {
  208. lr.SetPosition(0, jointObj.localPosition);
  209. //lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
  210. lr.SetPosition(1, rescaledJoints[(int)_BoneMap[jt]]);
  211. // Coloring the line renderer
  212. //lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
  213. //lr.startColor = GetColorForState(sourceJoint.TrackingState);
  214. //lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
  215. }
  216. else
  217. {
  218. lr.enabled = false;
  219. }
  220. // Record joints to PlayerReplay.cs
  221. playerReplay.joints[(int)jt] = jointObj;
  222. }
  223. // Joint orientations
  224. //Kinect.Vector4[] orientation = new Kinect.Vector4[22];
  225. //orientation[0] = body.JointOrientations[Kinect.JointType.Head].Orientation;
  226. //orientation[1] = body.JointOrientations[Kinect.JointType.Neck].Orientation;
  227. //orientation[2] = body.JointOrientations[Kinect.JointType.SpineMid].Orientation;
  228. //orientation[3] = body.JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
  229. //orientation[4] = body.JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
  230. //orientation[5] = body.JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
  231. //orientation[6] = body.JointOrientations[Kinect.JointType.ElbowRight].Orientation;
  232. //orientation[7] = body.JointOrientations[Kinect.JointType.WristLeft].Orientation;
  233. //orientation[8] = body.JointOrientations[Kinect.JointType.WristRight].Orientation;
  234. //orientation[9] = body.JointOrientations[Kinect.JointType.HipLeft].Orientation;
  235. //orientation[10] = body.JointOrientations[Kinect.JointType.HipRight].Orientation;
  236. //orientation[11] = body.JointOrientations[Kinect.JointType.KneeLeft].Orientation;
  237. //orientation[12] = body.JointOrientations[Kinect.JointType.KneeRight].Orientation;
  238. //orientation[13] = body.JointOrientations[Kinect.JointType.SpineBase].Orientation;
  239. //orientation[14] = body.JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
  240. //orientation[15] = body.JointOrientations[Kinect.JointType.AnkleRight].Orientation;
  241. //orientation[16] = body.JointOrientations[Kinect.JointType.FootLeft].Orientation;
  242. //orientation[17] = body.JointOrientations[Kinect.JointType.FootRight].Orientation;
  243. //orientation[18] = body.JointOrientations[Kinect.JointType.HandLeft].Orientation;
  244. //orientation[19] = body.JointOrientations[Kinect.JointType.HandRight].Orientation;
  245. //orientation[20] = body.JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
  246. //orientation[21] = body.JointOrientations[Kinect.JointType.ThumbRight].Orientation;
  247. //for (int j = 0; j < 22; j++)
  248. //{
  249. // //joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
  250. //}
  251. }
  252. private static Color GetColorForState(Kinect.TrackingState state)
  253. {
  254. switch (state)
  255. {
  256. case Kinect.TrackingState.Tracked:
  257. return Color.green;
  258. case Kinect.TrackingState.Inferred:
  259. return Color.red;
  260. default:
  261. return Color.black;
  262. }
  263. }
  264. private Vector3 GetVector3FromJoint(Kinect.Joint joint)
  265. {
  266. //return new Vector3(joint.Position.X * -10, joint.Position.Y * 10, joint.Position.Z * 10);
  267. return new Vector3(joint.Position.X * -1, joint.Position.Y, joint.Position.Z);
  268. }
  269. private Quaternion ConvertKinectOrientationToUnity(Kinect.Vector4 orientation)
  270. {
  271. Quaternion orientationUnity;
  272. orientationUnity.x = -orientation.X;
  273. orientationUnity.y = orientation.Y;
  274. orientationUnity.z = orientation.Z;
  275. orientationUnity.w = orientation.W;
  276. return orientationUnity;
  277. }
  278. // TODO: need to test
  279. private Vector3[] RescaleJoints(Vector3[] initJoints)
  280. {
  281. Vector3[] rescaledJoints = new Vector3[25];
  282. rescaledJoints[(int)Kinect.JointType.SpineBase] = Vector3.zero;
  283. // Lower part
  284. rescaledJoints[(int)Kinect.JointType.HipLeft] = GetTargetJointWithDesiredLength(
  285. initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipLeft], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];
  286. rescaledJoints[(int)Kinect.JointType.HipRight] = GetTargetJointWithDesiredLength(
  287. initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipRight], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];
  288. rescaledJoints[(int)Kinect.JointType.KneeLeft] = GetTargetJointWithDesiredLength(
  289. initJoints[(int)Kinect.JointType.HipLeft], initJoints[(int)Kinect.JointType.KneeLeft], 0.25f) + rescaledJoints[(int)Kinect.JointType.HipLeft];
  290. rescaledJoints[(int)Kinect.JointType.KneeRight] = GetTargetJointWithDesiredLength(
  291. initJoints[(int)Kinect.JointType.HipRight], initJoints[(int)Kinect.JointType.KneeRight], 0.25f) + rescaledJoints[(int)Kinect.JointType.HipRight];
  292. rescaledJoints[(int)Kinect.JointType.AnkleLeft] = GetTargetJointWithDesiredLength(
  293. initJoints[(int)Kinect.JointType.KneeLeft], initJoints[(int)Kinect.JointType.AnkleLeft], 0.4f) + rescaledJoints[(int)Kinect.JointType.KneeLeft];
  294. rescaledJoints[(int)Kinect.JointType.AnkleRight] = GetTargetJointWithDesiredLength(
  295. initJoints[(int)Kinect.JointType.KneeRight], initJoints[(int)Kinect.JointType.AnkleRight], 0.4f) + rescaledJoints[(int)Kinect.JointType.KneeRight];
  296. rescaledJoints[(int)Kinect.JointType.FootLeft] = GetTargetJointWithDesiredLength(
  297. initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleLeft];
  298. rescaledJoints[(int)Kinect.JointType.FootRight] = GetTargetJointWithDesiredLength(
  299. initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleRight];
  300. // Upper part
  301. rescaledJoints[(int)Kinect.JointType.SpineMid] = GetTargetJointWithDesiredLength(
  302. initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.SpineMid], 0.3f) + rescaledJoints[(int)Kinect.JointType.SpineBase];
  303. rescaledJoints[(int)Kinect.JointType.SpineShoulder] = GetTargetJointWithDesiredLength(
  304. initJoints[(int)Kinect.JointType.SpineMid], initJoints[(int)Kinect.JointType.SpineShoulder], 0.2f) + rescaledJoints[(int)Kinect.JointType.SpineMid];
  305. rescaledJoints[(int)Kinect.JointType.ShoulderLeft] = GetTargetJointWithDesiredLength(
  306. initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderLeft], 0.15f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
  307. rescaledJoints[(int)Kinect.JointType.ShoulderRight] = GetTargetJointWithDesiredLength(
  308. initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderRight], 0.15f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
  309. rescaledJoints[(int)Kinect.JointType.ElbowLeft] = GetTargetJointWithDesiredLength(
  310. initJoints[(int)Kinect.JointType.ShoulderLeft], initJoints[(int)Kinect.JointType.ElbowLeft], 0.25f) + rescaledJoints[(int)Kinect.JointType.ShoulderLeft];
  311. rescaledJoints[(int)Kinect.JointType.ElbowRight] = GetTargetJointWithDesiredLength(
  312. initJoints[(int)Kinect.JointType.ShoulderRight], initJoints[(int)Kinect.JointType.ElbowRight], 0.25f) + rescaledJoints[(int)Kinect.JointType.ShoulderRight];
  313. rescaledJoints[(int)Kinect.JointType.WristLeft] = GetTargetJointWithDesiredLength(
  314. initJoints[(int)Kinect.JointType.ElbowLeft], initJoints[(int)Kinect.JointType.WristLeft], 0.2f) + rescaledJoints[(int)Kinect.JointType.ElbowLeft];
  315. rescaledJoints[(int)Kinect.JointType.WristRight] = GetTargetJointWithDesiredLength(
  316. initJoints[(int)Kinect.JointType.ElbowRight], initJoints[(int)Kinect.JointType.WristRight], 0.2f) + rescaledJoints[(int)Kinect.JointType.ElbowRight];
  317. rescaledJoints[(int)Kinect.JointType.Neck] = GetTargetJointWithDesiredLength(
  318. initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.Neck], 0.07f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
  319. return rescaledJoints;
  320. }
  321. private Vector3 GetTargetJointWithDesiredLength(Vector3 source, Vector3 target, float desiredLength)
  322. {
  323. Vector3 sourceToTarget = target - source;
  324. float length = sourceToTarget.magnitude;
  325. return sourceToTarget * desiredLength / length;
  326. }
  327. }