tarukosu/KinectReceiver.cs

## 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;
    }
}
	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;
	}
	}