Update BodySourceView.cs

Assets/KinectView/Scripts/BodySourceView.cs

@@ -1,50 +1,49 @@
-using UnityEngine;
-using System.Collections;
-using System.Collections.Generic;
+using System.Collections.Generic;
+using UnityEngine;
 using Kinect = Windows.Kinect;
 
-public class BodySourceView : MonoBehaviour 
+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 () 
+    void Update()
     {
         if (BodySourceManager == null)
         {
@@ -56,187 +55,158 @@ public class BodySourceView : MonoBehaviour
         {
             return;
         }
-        
+
         Kinect.Body[] data = _BodyManager.GetData();
         if (data == null)
         {
             return;
         }
-        
+
         List<ulong> trackedIds = new List<ulong>();
-        foreach(var body in data)
+        foreach (var body in data)
         {
             if (body == null)
             {
                 continue;
-              }
-                
-            if(body.IsTracked)
+            }
+
+            if (body.IsTracked)
             {
-                trackedIds.Add (body.TrackingId);
+                trackedIds.Add(body.TrackingId);
             }
         }
-        
+
         List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
-        
+
         // First delete untracked bodies
-        foreach(ulong trackingId in knownIds)
+        foreach (ulong trackingId in knownIds)
         {
-            if(!trackedIds.Contains(trackingId))
+            if (!trackedIds.Contains(trackingId))
             {
                 Destroy(_Bodies[trackingId]);
                 _Bodies.Remove(trackingId);
             }
         }
 
-        foreach(var body in data)
+        foreach (var body in data)
         {
             if (body == null)
             {
                 continue;
             }
-            
-            if(body.IsTracked)
+
+            if (body.IsTracked)
             {
-                if(!_Bodies.ContainsKey(body.TrackingId))
+                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.SetVertexCount(2);
+            lr.positionCount = 2;
             lr.material = BoneMaterial;
-            lr.SetWidth(0.05f, 0.05f);
-            
+            //lr.SetWidth(0.05f, 0.05f);
+            lr.startWidth = 0.05f;
+            lr.endWidth = 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))
+
+            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)
+            if (targetJoint.HasValue)
             {
                 lr.SetPosition(0, jointObj.localPosition);
                 lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
-                lr.SetColors(GetColorForState (sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
+                //lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
+                lr.startColor = GetColorForState(sourceJoint.TrackingState);
+                lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
             }
             else
             {
                 lr.enabled = false;
             }
         }
+
+        // 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.Tracked:
+                return Color.green;
 
-        case Kinect.TrackingState.Inferred:
-            return Color.red;
+            case Kinect.TrackingState.Inferred:
+                return Color.red;
 
-        default:
-            return Color.black;
+            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);