123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292 |
- using System.Collections.Generic;
- using UnityEngine;
- using Kinect = Windows.Kinect;
- using Valve.VR;
- public class BodySourceView : MonoBehaviour
- {
- public Transform[] joints;
- public Material BoneMaterial;
- public GameObject BodySourceManager;
- public Transform cameraRig;
- public PlayerReplay playerReplay;
- public ModeController modeController;
- public bool wristLeftLate;
- public bool wristRightLate;
- private Dictionary<ulong, GameObject> _Bodies = new Dictionary<ulong, GameObject>();
- private BodySourceManager _BodyManager;
- private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
- {
- { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
- { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
- { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
- { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },
- { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
- { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
- { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
- { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },
- { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
- { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
- { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
- { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
- { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
- { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },
- { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
- { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
- { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
- { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
- { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
- { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },
- { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
- { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
- { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
- { Kinect.JointType.Neck, Kinect.JointType.Head },
- };
- void Update()
- {
- if (BodySourceManager == null)
- {
- return;
- }
- _BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
- if (_BodyManager == null)
- {
- return;
- }
- Kinect.Body[] data = _BodyManager.GetData();
- if (data == null)
- {
- return;
- }
- List<ulong> trackedIds = new List<ulong>();
- foreach (var body in data)
- {
- if (body == null)
- {
- continue;
- }
- if (body.IsTracked)
- {
- trackedIds.Add(body.TrackingId);
- }
- }
- List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
- // First delete untracked bodies
- foreach (ulong trackingId in knownIds)
- {
- if (!trackedIds.Contains(trackingId))
- {
- Destroy(_Bodies[trackingId]);
- _Bodies.Remove(trackingId);
- }
- }
- foreach (var body in data)
- {
- if (body == null)
- {
- continue;
- }
- if (body.IsTracked)
- {
- if (!_Bodies.ContainsKey(body.TrackingId))
- {
- _Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
- }
- RefreshBodyObject(body, _Bodies[body.TrackingId]);
- }
- }
- }
- private GameObject CreateBodyObject(ulong id)
- {
- GameObject body = new GameObject("Body:" + id);
- body.transform.parent = gameObject.transform;
- for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
- {
- GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
- // Give WristLeft and WristRight rigidbody
- if (jt == Kinect.JointType.WristLeft)
- {
- jointObj.tag = "WristLeft";
- Rigidbody rb = jointObj.AddComponent<Rigidbody>();
- rb.useGravity = false;
- rb.isKinematic = true;
- } else if (jt == Kinect.JointType.WristRight)
- {
- jointObj.tag = "WristRight";
- Rigidbody rb = jointObj.AddComponent<Rigidbody>();
- rb.useGravity = false;
- rb.isKinematic = true;
- }
- LineRenderer lr = jointObj.AddComponent<LineRenderer>();
- //lr.SetVertexCount(2);
- lr.positionCount = 2;
- lr.material = BoneMaterial;
- //lr.SetWidth(0.05f, 0.05f);
- lr.startWidth = 0.3f;
- lr.endWidth = 0.3f;
- jointObj.transform.localScale = new Vector3(0.3f, 0.3f, 0.3f);
- jointObj.name = jt.ToString();
- jointObj.transform.parent = body.transform;
- }
- return body;
- }
- private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
- {
- for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
- {
- Kinect.Joint sourceJoint = body.Joints[jt];
- Kinect.Joint? targetJoint = null;
- if (_BoneMap.ContainsKey(jt))
- {
- targetJoint = body.Joints[_BoneMap[jt]];
- }
- Transform jointObj = bodyObject.transform.Find(jt.ToString());
- jointObj.localPosition = GetVector3FromJoint(sourceJoint);
-
- if (jt == Kinect.JointType.Head)
- {
- // Make head joint invisible
- jointObj.GetComponent<Renderer>().enabled = false;
- // Make camera follow the head
- if (modeController.perspective == ModeController.Perspective.FirstPersonPerspective)
- {
- cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y - 3, jointObj.localPosition.z - 0.5f);
- } else
- {
- cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y - 3, jointObj.localPosition.z + 3);
- }
- }
- // Make these joints invisible
- if (jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight
- || jt == Kinect.JointType.HandLeft || jt == Kinect.JointType.HandRight
- || jt == Kinect.JointType.HandTipLeft || jt == Kinect.JointType.HandTipRight)
- {
- jointObj.GetComponent<Renderer>().enabled = false;
- }
- // WristLeft and WristRight do something if not following the motions
- if ((jt == Kinect.JointType.WristLeft && wristLeftLate) || (jt == Kinect.JointType.WristRight && wristRightLate))
- {
- if (modeController.feedback == ModeController.Feedback.ColorFeedback)
- {
- // Change to red color
- jointObj.GetComponent<Renderer>().material.color = new Color(1, 0, 0);
- } else if (modeController.feedback == ModeController.Feedback.HapticFeedback)
- {
- // TODO: Add haptic feedback need to test
- SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftHand].Execute(0, 1, 10, 1);
- SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightHand].Execute(0, 1, 10, 1);
- }
- }
- LineRenderer lr = jointObj.GetComponent<LineRenderer>();
- if (targetJoint.HasValue && jt != Kinect.JointType.Neck && jt != Kinect.JointType.ThumbLeft && jt != Kinect.JointType.ThumbRight
- && jt != Kinect.JointType.HandLeft && jt != Kinect.JointType.HandRight
- && jt != Kinect.JointType.HandTipLeft && jt != Kinect.JointType.HandTipRight) // Do not make LineRenderer for these joints
- {
- lr.SetPosition(0, jointObj.localPosition);
- lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
- // Coloring the line renderer
- //lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
- //lr.startColor = GetColorForState(sourceJoint.TrackingState);
- //lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
- }
- else
- {
- lr.enabled = false;
- }
- // Record joints to PlayerReplay.cs
- playerReplay.joints[(int)jt] = jointObj;
- }
- // Joint orientations
- //Kinect.Vector4[] orientation = new Kinect.Vector4[22];
- //orientation[0] = body.JointOrientations[Kinect.JointType.Head].Orientation;
- //orientation[1] = body.JointOrientations[Kinect.JointType.Neck].Orientation;
- //orientation[2] = body.JointOrientations[Kinect.JointType.SpineMid].Orientation;
- //orientation[3] = body.JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
- //orientation[4] = body.JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
- //orientation[5] = body.JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
- //orientation[6] = body.JointOrientations[Kinect.JointType.ElbowRight].Orientation;
- //orientation[7] = body.JointOrientations[Kinect.JointType.WristLeft].Orientation;
- //orientation[8] = body.JointOrientations[Kinect.JointType.WristRight].Orientation;
- //orientation[9] = body.JointOrientations[Kinect.JointType.HipLeft].Orientation;
- //orientation[10] = body.JointOrientations[Kinect.JointType.HipRight].Orientation;
- //orientation[11] = body.JointOrientations[Kinect.JointType.KneeLeft].Orientation;
- //orientation[12] = body.JointOrientations[Kinect.JointType.KneeRight].Orientation;
- //orientation[13] = body.JointOrientations[Kinect.JointType.SpineBase].Orientation;
- //orientation[14] = body.JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
- //orientation[15] = body.JointOrientations[Kinect.JointType.AnkleRight].Orientation;
- //orientation[16] = body.JointOrientations[Kinect.JointType.FootLeft].Orientation;
- //orientation[17] = body.JointOrientations[Kinect.JointType.FootRight].Orientation;
- //orientation[18] = body.JointOrientations[Kinect.JointType.HandLeft].Orientation;
- //orientation[19] = body.JointOrientations[Kinect.JointType.HandRight].Orientation;
- //orientation[20] = body.JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
- //orientation[21] = body.JointOrientations[Kinect.JointType.ThumbRight].Orientation;
- //for (int j = 0; j < 22; j++)
- //{
- // //joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
- //}
- }
- private static Color GetColorForState(Kinect.TrackingState state)
- {
- switch (state)
- {
- case Kinect.TrackingState.Tracked:
- return Color.green;
- case Kinect.TrackingState.Inferred:
- return Color.red;
- default:
- return Color.black;
- }
- }
- private static Vector3 GetVector3FromJoint(Kinect.Joint joint)
- {
- return new Vector3(joint.Position.X * -10, joint.Position.Y * 10 + 13.3f, joint.Position.Z * 10);
- }
- private Quaternion ConvertKinectOrientationToUnity(Kinect.Vector4 orientation)
- {
- Quaternion orientationUnity;
- orientationUnity.x = -orientation.X;
- orientationUnity.y = orientation.Y;
- orientationUnity.z = orientation.Z;
- orientationUnity.w = orientation.W;
- return orientationUnity;
- }
- }
|