thesis-motion-guidance/Assets/KinectView/Scripts/BodySourceView.cs

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Kinect = Windows.Kinect;

public class BodySourceView : MonoBehaviour 
{
    public Transform[] joints;
    public Material BoneMaterial;
    public GameObject BodySourceManager;
    
    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]);
            }
        }
        
        // Joint orientations
        Kinect.Vector4[] orientation = new Kinect.Vector4[22];
        //for(int i = 0; i < data.Length; i++)
        //{
        //    orientation[0] = data[i].JointOrientations[Kinect.JointType.Head].Orientation;
        //    orientation[1] = data[i].JointOrientations[Kinect.JointType.Neck].Orientation;
        //    orientation[2] = data[i].JointOrientations[Kinect.JointType.SpineMid].Orientation;
        //    orientation[3] = data[i].JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
        //    orientation[4] = data[i].JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
        //    orientation[5] = data[i].JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
        //    orientation[6] = data[i].JointOrientations[Kinect.JointType.ElbowRight].Orientation;
        //    orientation[7] = data[i].JointOrientations[Kinect.JointType.WristLeft].Orientation;
        //    orientation[8] = data[i].JointOrientations[Kinect.JointType.WristRight].Orientation;
        //    orientation[9] = data[i].JointOrientations[Kinect.JointType.HipLeft].Orientation;
        //    orientation[10] = data[i].JointOrientations[Kinect.JointType.HipRight].Orientation;
        //    orientation[11] = data[i].JointOrientations[Kinect.JointType.KneeLeft].Orientation;
        //    orientation[12] = data[i].JointOrientations[Kinect.JointType.KneeRight].Orientation;
        //    orientation[13] = data[i].JointOrientations[Kinect.JointType.SpineBase].Orientation;
        //    orientation[14] = data[i].JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
        //    orientation[15] = data[i].JointOrientations[Kinect.JointType.AnkleRight].Orientation;
        //    orientation[16] = data[i].JointOrientations[Kinect.JointType.FootLeft].Orientation;
        //    orientation[17] = data[i].JointOrientations[Kinect.JointType.FootRight].Orientation;
        //    orientation[18] = data[i].JointOrientations[Kinect.JointType.HandLeft].Orientation;
        //    orientation[19] = data[i].JointOrientations[Kinect.JointType.HandRight].Orientation;
        //    orientation[20] = data[i].JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
        //    orientation[21] = data[i].JointOrientations[Kinect.JointType.ThumbRight].Orientation;

        //    for (int j = 0; j < 22; j++)
        //    {
        //        joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
        //    }
        //}

        orientation[0] = data[0].JointOrientations[Kinect.JointType.Head].Orientation;
        orientation[1] = data[0].JointOrientations[Kinect.JointType.Neck].Orientation;
        orientation[2] = data[0].JointOrientations[Kinect.JointType.SpineMid].Orientation;
        orientation[3] = data[0].JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
        orientation[4] = data[0].JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
        orientation[5] = data[0].JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
        orientation[6] = data[0].JointOrientations[Kinect.JointType.ElbowRight].Orientation;
        orientation[7] = data[0].JointOrientations[Kinect.JointType.WristLeft].Orientation;
        orientation[8] = data[0].JointOrientations[Kinect.JointType.WristRight].Orientation;
        orientation[9] = data[0].JointOrientations[Kinect.JointType.HipLeft].Orientation;
        orientation[10] = data[0].JointOrientations[Kinect.JointType.HipRight].Orientation;
        orientation[11] = data[0].JointOrientations[Kinect.JointType.KneeLeft].Orientation;
        orientation[12] = data[0].JointOrientations[Kinect.JointType.KneeRight].Orientation;
        orientation[13] = data[0].JointOrientations[Kinect.JointType.SpineBase].Orientation;
        orientation[14] = data[0].JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
        orientation[15] = data[0].JointOrientations[Kinect.JointType.AnkleRight].Orientation;
        orientation[16] = data[0].JointOrientations[Kinect.JointType.FootLeft].Orientation;
        orientation[17] = data[0].JointOrientations[Kinect.JointType.FootRight].Orientation;
        orientation[18] = data[0].JointOrientations[Kinect.JointType.HandLeft].Orientation;
        orientation[19] = data[0].JointOrientations[Kinect.JointType.HandRight].Orientation;
        orientation[20] = data[0].JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
        orientation[21] = data[0].JointOrientations[Kinect.JointType.ThumbRight].Orientation;

        for (int j = 0; j < 22; j++)
        {
            joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
        }

        //print("wrist right: " + joints[8].rotation);

    }

    private GameObject CreateBodyObject(ulong id)
    {
        GameObject body = new GameObject("Body:" + id);
        
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
            
            LineRenderer lr = jointObj.AddComponent<LineRenderer>();
            lr.SetVertexCount(2);
            lr.material = BoneMaterial;
            lr.SetWidth(0.05f, 0.05f);
            
            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);
            
            LineRenderer lr = jointObj.GetComponent<LineRenderer>();
            if(targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.localPosition);
                lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                lr.SetColors(GetColorForState (sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
            }
            else
            {
                lr.enabled = false;
            }
        }
    }
    
    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, 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;
    }
}