BodySourceView.cs 9.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237
  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 Material BoneMaterial;
  8. public GameObject BodySourceManager;
  9. public Transform cameraRig;
  10. public PlayerReplay playerReplay;
  11. private Dictionary<ulong, GameObject> _Bodies = new Dictionary<ulong, GameObject>();
  12. private BodySourceManager _BodyManager;
  13. private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
  14. {
  15. { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
  16. { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
  17. { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
  18. { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },
  19. { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
  20. { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
  21. { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
  22. { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },
  23. { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
  24. { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
  25. { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
  26. { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
  27. { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
  28. { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },
  29. { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
  30. { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
  31. { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
  32. { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
  33. { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
  34. { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },
  35. { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
  36. { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
  37. { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
  38. { Kinect.JointType.Neck, Kinect.JointType.Head },
  39. };
  40. void Update()
  41. {
  42. if (BodySourceManager == null)
  43. {
  44. return;
  45. }
  46. _BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
  47. if (_BodyManager == null)
  48. {
  49. return;
  50. }
  51. Kinect.Body[] data = _BodyManager.GetData();
  52. if (data == null)
  53. {
  54. return;
  55. }
  56. List<ulong> trackedIds = new List<ulong>();
  57. foreach (var body in data)
  58. {
  59. if (body == null)
  60. {
  61. continue;
  62. }
  63. if (body.IsTracked)
  64. {
  65. trackedIds.Add(body.TrackingId);
  66. }
  67. }
  68. List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
  69. // First delete untracked bodies
  70. foreach (ulong trackingId in knownIds)
  71. {
  72. if (!trackedIds.Contains(trackingId))
  73. {
  74. Destroy(_Bodies[trackingId]);
  75. _Bodies.Remove(trackingId);
  76. }
  77. }
  78. foreach (var body in data)
  79. {
  80. if (body == null)
  81. {
  82. continue;
  83. }
  84. if (body.IsTracked)
  85. {
  86. if (!_Bodies.ContainsKey(body.TrackingId))
  87. {
  88. _Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
  89. }
  90. RefreshBodyObject(body, _Bodies[body.TrackingId]);
  91. }
  92. }
  93. }
  94. private GameObject CreateBodyObject(ulong id)
  95. {
  96. GameObject body = new GameObject("Body:" + id);
  97. for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
  98. {
  99. GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
  100. LineRenderer lr = jointObj.AddComponent<LineRenderer>();
  101. //lr.SetVertexCount(2);
  102. lr.positionCount = 2;
  103. lr.material = BoneMaterial;
  104. //lr.SetWidth(0.05f, 0.05f);
  105. lr.startWidth = 0.3f;
  106. lr.endWidth = 0.3f;
  107. jointObj.transform.localScale = new Vector3(0.3f, 0.3f, 0.3f);
  108. jointObj.name = jt.ToString();
  109. jointObj.transform.parent = body.transform;
  110. }
  111. return body;
  112. }
  113. private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
  114. {
  115. for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
  116. {
  117. Kinect.Joint sourceJoint = body.Joints[jt];
  118. Kinect.Joint? targetJoint = null;
  119. if (_BoneMap.ContainsKey(jt))
  120. {
  121. targetJoint = body.Joints[_BoneMap[jt]];
  122. }
  123. Transform jointObj = bodyObject.transform.Find(jt.ToString());
  124. jointObj.localPosition = GetVector3FromJoint(sourceJoint);
  125. if (jt == Kinect.JointType.Head)
  126. {
  127. // Make head joint invisible
  128. jointObj.GetComponent<Renderer>().enabled = false;
  129. // Make camera follow the head
  130. cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y - 2, jointObj.localPosition.z + 2);
  131. }
  132. LineRenderer lr = jointObj.GetComponent<LineRenderer>();
  133. if (targetJoint.HasValue && jt != Kinect.JointType.Neck) // Do not make LineRenderer from neck to head
  134. {
  135. lr.SetPosition(0, jointObj.localPosition);
  136. lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
  137. //lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
  138. lr.startColor = GetColorForState(sourceJoint.TrackingState);
  139. lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
  140. }
  141. else
  142. {
  143. lr.enabled = false;
  144. }
  145. // Record joints to PlayerReplay.cs
  146. playerReplay.joints[(int)jt] = jointObj;
  147. }
  148. // Joint orientations
  149. //Kinect.Vector4[] orientation = new Kinect.Vector4[22];
  150. //orientation[0] = body.JointOrientations[Kinect.JointType.Head].Orientation;
  151. //orientation[1] = body.JointOrientations[Kinect.JointType.Neck].Orientation;
  152. //orientation[2] = body.JointOrientations[Kinect.JointType.SpineMid].Orientation;
  153. //orientation[3] = body.JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
  154. //orientation[4] = body.JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
  155. //orientation[5] = body.JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
  156. //orientation[6] = body.JointOrientations[Kinect.JointType.ElbowRight].Orientation;
  157. //orientation[7] = body.JointOrientations[Kinect.JointType.WristLeft].Orientation;
  158. //orientation[8] = body.JointOrientations[Kinect.JointType.WristRight].Orientation;
  159. //orientation[9] = body.JointOrientations[Kinect.JointType.HipLeft].Orientation;
  160. //orientation[10] = body.JointOrientations[Kinect.JointType.HipRight].Orientation;
  161. //orientation[11] = body.JointOrientations[Kinect.JointType.KneeLeft].Orientation;
  162. //orientation[12] = body.JointOrientations[Kinect.JointType.KneeRight].Orientation;
  163. //orientation[13] = body.JointOrientations[Kinect.JointType.SpineBase].Orientation;
  164. //orientation[14] = body.JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
  165. //orientation[15] = body.JointOrientations[Kinect.JointType.AnkleRight].Orientation;
  166. //orientation[16] = body.JointOrientations[Kinect.JointType.FootLeft].Orientation;
  167. //orientation[17] = body.JointOrientations[Kinect.JointType.FootRight].Orientation;
  168. //orientation[18] = body.JointOrientations[Kinect.JointType.HandLeft].Orientation;
  169. //orientation[19] = body.JointOrientations[Kinect.JointType.HandRight].Orientation;
  170. //orientation[20] = body.JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
  171. //orientation[21] = body.JointOrientations[Kinect.JointType.ThumbRight].Orientation;
  172. //for (int j = 0; j < 22; j++)
  173. //{
  174. // //joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
  175. //}
  176. }
  177. private static Color GetColorForState(Kinect.TrackingState state)
  178. {
  179. switch (state)
  180. {
  181. case Kinect.TrackingState.Tracked:
  182. return Color.green;
  183. case Kinect.TrackingState.Inferred:
  184. return Color.red;
  185. default:
  186. return Color.black;
  187. }
  188. }
  189. private static Vector3 GetVector3FromJoint(Kinect.Joint joint)
  190. {
  191. return new Vector3(joint.Position.X * -10, joint.Position.Y * 10 + 10, joint.Position.Z * 10);
  192. }
  193. private Quaternion ConvertKinectOrientationToUnity(Kinect.Vector4 orientation)
  194. {
  195. Quaternion orientationUnity;
  196. orientationUnity.x = -orientation.X;
  197. orientationUnity.y = orientation.Y;
  198. orientationUnity.z = orientation.Z;
  199. orientationUnity.w = orientation.W;
  200. return orientationUnity;
  201. }
  202. }