thesis-motion-guidance/Assets/Scripts/HelperScript.cs

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

public static class HelperScript
{

    public static 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 },
    };

    public static Vector3[] ConvertJointsDataToVector3Array(JointsData jd)
    {
        Vector3[] joints = new Vector3[jd.jointsPositionsX.Count];
        for (int i = 0; i < joints.Length; i++)
        {
            joints[i] = new Vector3(jd.jointsPositionsX[i], jd.jointsPositionsY[i], jd.jointsPositionsZ[i]);
        }
        return joints;
    }

    public static Vector3[] RescaleJoints(Vector3[] initJoints)
    {
        Vector3[] rescaledJoints = new Vector3[25];

        rescaledJoints[(int)Kinect.JointType.SpineBase] = Vector3.zero;

        // Lower part
        rescaledJoints[(int)Kinect.JointType.HipLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipLeft], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];
        rescaledJoints[(int)Kinect.JointType.HipRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipRight], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.KneeLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipLeft], initJoints[(int)Kinect.JointType.KneeLeft], 0.38f) + rescaledJoints[(int)Kinect.JointType.HipLeft];
        rescaledJoints[(int)Kinect.JointType.KneeRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipRight], initJoints[(int)Kinect.JointType.KneeRight], 0.38f) + rescaledJoints[(int)Kinect.JointType.HipRight];

        rescaledJoints[(int)Kinect.JointType.AnkleLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeLeft], initJoints[(int)Kinect.JointType.AnkleLeft], 0.35f) + rescaledJoints[(int)Kinect.JointType.KneeLeft];
        rescaledJoints[(int)Kinect.JointType.AnkleRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeRight], initJoints[(int)Kinect.JointType.AnkleRight], 0.35f) + rescaledJoints[(int)Kinect.JointType.KneeRight];

        rescaledJoints[(int)Kinect.JointType.FootLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleLeft] + new Vector3(0, 0.03f, 0);
        //initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleLeft];
        rescaledJoints[(int)Kinect.JointType.FootRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleRight] + new Vector3(0, 0.03f, 0);
        //initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleRight];

        // Upper part
        rescaledJoints[(int)Kinect.JointType.SpineMid] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.SpineMid], 0.2f) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.SpineShoulder] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineMid], initJoints[(int)Kinect.JointType.SpineShoulder], 0.2f) + rescaledJoints[(int)Kinect.JointType.SpineMid];

        rescaledJoints[(int)Kinect.JointType.ShoulderLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderLeft], 0.12f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
        rescaledJoints[(int)Kinect.JointType.ShoulderRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderRight], 0.12f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        rescaledJoints[(int)Kinect.JointType.ElbowLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderLeft], initJoints[(int)Kinect.JointType.ElbowLeft], 0.28f) + rescaledJoints[(int)Kinect.JointType.ShoulderLeft];
        rescaledJoints[(int)Kinect.JointType.ElbowRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderRight], initJoints[(int)Kinect.JointType.ElbowRight], 0.28f) + rescaledJoints[(int)Kinect.JointType.ShoulderRight];

        rescaledJoints[(int)Kinect.JointType.WristLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowLeft], initJoints[(int)Kinect.JointType.WristLeft], 0.28f) + rescaledJoints[(int)Kinect.JointType.ElbowLeft];
        rescaledJoints[(int)Kinect.JointType.WristRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowRight], initJoints[(int)Kinect.JointType.WristRight], 0.28f) + rescaledJoints[(int)Kinect.JointType.ElbowRight];

        rescaledJoints[(int)Kinect.JointType.Neck] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.Neck], 0.1f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        return rescaledJoints;
    }

    private static Vector3 GetTargetJointWithDesiredLength(Vector3 source, Vector3 target, float desiredLength)
    {
        Vector3 sourceToTarget = target - source;
        float length = sourceToTarget.magnitude;
        return sourceToTarget * desiredLength / length;
    }
}