BodySourceView.cs 15 KB

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