Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
KinectReceiver.cs
using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using UnetLLAPISample;
using UnityEngine;
using Kinect = Windows.Kinect;
[Serializable]
public struct SimpleBodies
{
public List<SimpleBody> Bodies;
}
[Serializable]
public struct SimpleBody
{
public List<SimpleJoint> Joints;
}
[Serializable]
public struct SimpleJoint
{
public Vector3 Position;
public int TrackingState;
}
public class Body
{
public Dictionary<Kinect.JointType, Joint> Joints;
}
public class Joint
{
public Vector3 Position;
public Kinect.TrackingState TrackingState;
}
public class KinectReceiver : MonoBehaviour
{
public LLAPINetworkManager NetworkManager;
public GameObject JointPrefab;
public Body body;
public event Action<Body> BodyUpdated = delegate (Body b) { };
private Body localBody;
private GameObject bodyObject;
private void Awake()
{
body = CreateBody();
localBody = CreateBody();
bodyObject = CreateBodyObject();
}
void Start()
{
NetworkManager.OnDataReceived += OnDataReceived;
}
Body CreateBody()
{
var body = new Body();
var joints = new Dictionary<Kinect.JointType, Joint>();
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
var joint = new Joint();
joint.Position = Vector3.zero;
joint.TrackingState = Kinect.TrackingState.NotTracked;
joints.Add(jt, joint);
}
body.Joints = joints;
return body;
}
void OnDataReceived(object o, LLAPINetworkEventArgs e)
{
SimpleBody simpleBody = new SimpleBody();
string output;
var data = e.data;
using (var inStream = new MemoryStream(data))
using (var bigStream = new System.IO.Compression.GZipStream(inStream, System.IO.Compression.CompressionMode.Decompress))
using (var reader = new StreamReader(bigStream))
{
output = reader.ReadToEnd();
}
simpleBody = JsonUtility.FromJson<SimpleBody>(output);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
var j = localBody.Joints[jt];
var sj = simpleBody.Joints[(int)jt];
j.Position = sj.Position;
j.TrackingState = (Kinect.TrackingState)sj.TrackingState;
}
DisplayBodyObject();
UpdateBody();
}
GameObject CreateBodyObject()
{
GameObject bodyObj = new GameObject("Body");
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
GameObject jointObj = Instantiate(JointPrefab);
jointObj.name = jt.ToString();
jointObj.transform.parent = bodyObj.transform;
}
return bodyObj;
}
void DisplayBodyObject()
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Joint sourceJoint = localBody.Joints[jt];
Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
jointObj.localPosition = sourceJoint.Position;
}
}
void UpdateBody()
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
var bodyJoint = body.Joints[jt];
var localBodyJoint = localBody.Joints[jt];
Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
bodyJoint.Position = jointObj.transform.position;
bodyJoint.TrackingState = localBodyJoint.TrackingState;
}
BodyUpdated(body);
}
public void Calibrate()
{
var headPos = localBody.Joints[Kinect.JointType.Head].Position;
var rarmPos = localBody.Joints[Kinect.JointType.ElbowRight].Position;
var larmPos = localBody.Joints[Kinect.JointType.ElbowLeft].Position;
var cameraOffset = -0.15f * Camera.main.transform.forward;
var cameraPos = Camera.main.transform.position + cameraOffset;
var cameraRotation = Camera.main.transform.rotation;
var cameraRotationY = Quaternion.AngleAxis(cameraRotation.eulerAngles.y, Vector3.up);
var lToR = rarmPos - larmPos;
var angle = -Mathf.Atan2(lToR.z, lToR.x) * Mathf.Rad2Deg;
var headRotation = Quaternion.AngleAxis(angle, Vector3.up);
var kinect2Head = Matrix4x4.TRS(headPos, headRotation, Vector3.one);
var origin2Head = Matrix4x4.TRS(cameraPos, cameraRotationY, Vector3.one);
var origin2Kinect = origin2Head * kinect2Head.inverse;
Vector3 position = origin2Kinect.GetColumn(3);
Quaternion rotation = Quaternion.LookRotation(
origin2Kinect.GetColumn(2),
origin2Kinect.GetColumn(1)
);
bodyObject.transform.position = position;
bodyObject.transform.rotation = rotation;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment