421 lines
16 KiB
C#
421 lines
16 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using UnityEngine.Events;
|
|
|
|
namespace com.rfilkov.kinect
|
|
{
|
|
/// <summary>
|
|
/// Kinect user manager is the component that tracks the users in front of the sensor.
|
|
/// </summary>
|
|
public class KinectUserManager : MonoBehaviour
|
|
{
|
|
|
|
[System.Serializable]
|
|
public class KinectUserEvent : UnityEvent<ulong, int> { }
|
|
|
|
|
|
/// <summary>
|
|
/// Fired when new user gets detected.
|
|
/// </summary>
|
|
//public event System.Action<ulong, int> OnUserAdded;
|
|
public KinectUserEvent OnUserAdded;
|
|
|
|
/// <summary>
|
|
/// Fired when user gets removed.
|
|
/// </summary>
|
|
//public event System.Action<ulong, int> OnUserRemoved;
|
|
public KinectUserEvent OnUserRemoved;
|
|
|
|
|
|
// List of all users
|
|
internal List<ulong> alUserIds = new List<ulong>();
|
|
internal Dictionary<ulong, int> dictUserIdToIndex = new Dictionary<ulong, int>();
|
|
internal ulong[] aUserIndexIds = new ulong[KinectInterop.Constants.MaxBodyCount];
|
|
internal Dictionary<ulong, float> dictUserIdToTime = new Dictionary<ulong, float>();
|
|
|
|
// Primary (first or closest) user ID
|
|
internal ulong liPrimaryUserId = 0;
|
|
|
|
// Calibration gesture data for each player
|
|
internal Dictionary<ulong, KinectGestureManager.GestureData> playerCalibrationData = new Dictionary<ulong, KinectGestureManager.GestureData>();
|
|
|
|
// reference to KM
|
|
private KinectManager kinectManager = null;
|
|
|
|
|
|
protected virtual void Start()
|
|
{
|
|
kinectManager = KinectManager.Instance;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Rearranges the user indices, according to the current criteria
|
|
/// </summary>
|
|
public virtual void RearrangeUserIndices(KinectManager.UserDetectionOrder userDetectionOrder)
|
|
{
|
|
|
|
if (userDetectionOrder != KinectManager.UserDetectionOrder.Appearance)
|
|
{
|
|
// get current user positions
|
|
Vector3[] userPos = new Vector3[aUserIndexIds.Length];
|
|
for (int i = 0; i < aUserIndexIds.Length; i++)
|
|
{
|
|
ulong userId = aUserIndexIds[i];
|
|
userPos[i] = userId != 0 ? kinectManager.GetUserPosition(userId) : Vector3.zero;
|
|
}
|
|
|
|
// bubble sort
|
|
bool reorderDone = false;
|
|
for (int i = aUserIndexIds.Length - 1; i >= 1; i--)
|
|
{
|
|
bool switchDone = false;
|
|
|
|
for (int j = 0; j < i; j++)
|
|
{
|
|
float userDist1 = 0f;
|
|
if (userDetectionOrder == KinectManager.UserDetectionOrder.Distance)
|
|
userDist1 = Mathf.Abs(userPos[j].x) + Mathf.Abs(userPos[j].z);
|
|
else if (userDetectionOrder == KinectManager.UserDetectionOrder.LeftToRight)
|
|
userDist1 = userPos[j].x;
|
|
|
|
if (Mathf.Abs(userDist1) < 0.01f)
|
|
userDist1 = 1000f; // far away
|
|
|
|
float userDist2 = 0f;
|
|
if (userDetectionOrder == KinectManager.UserDetectionOrder.Distance)
|
|
userDist2 = Mathf.Abs(userPos[j + 1].x) + Mathf.Abs(userPos[j + 1].z);
|
|
else if (userDetectionOrder == KinectManager.UserDetectionOrder.LeftToRight)
|
|
userDist2 = userPos[j + 1].x;
|
|
|
|
if (Mathf.Abs(userDist2) < 0.01f)
|
|
userDist2 = 1000f; // far away
|
|
|
|
if (userDist1 > userDist2)
|
|
{
|
|
// switch them
|
|
ulong tmpUserId = aUserIndexIds[j];
|
|
aUserIndexIds[j] = aUserIndexIds[j + 1];
|
|
aUserIndexIds[j + 1] = tmpUserId;
|
|
|
|
reorderDone = switchDone = true;
|
|
}
|
|
}
|
|
|
|
if (!switchDone) // check for sorted array
|
|
break;
|
|
}
|
|
|
|
if (reorderDone)
|
|
{
|
|
System.Text.StringBuilder sbUsersOrder = new System.Text.StringBuilder();
|
|
sbUsersOrder.Append("Users reindexed: ");
|
|
|
|
for (int i = 0; i < aUserIndexIds.Length; i++)
|
|
{
|
|
if (aUserIndexIds[i] != 0)
|
|
{
|
|
sbUsersOrder.Append(i).Append(":").Append(aUserIndexIds[i]).Append(" ");
|
|
}
|
|
}
|
|
|
|
Debug.Log(sbUsersOrder.ToString());
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
// Returns empty user slot for the given user Id
|
|
protected virtual int GetEmptyUserSlot(ulong userId, int bodyIndex, ref KinectInterop.BodyData[] alTrackedBodies, KinectManager.UserDetectionOrder userDetectionOrder)
|
|
{
|
|
// rearrange current users
|
|
RearrangeUserIndices(userDetectionOrder);
|
|
int uidIndex = -1;
|
|
|
|
if (userDetectionOrder != KinectManager.UserDetectionOrder.Appearance)
|
|
{
|
|
// add the new user, depending on the distance
|
|
Vector3 userPos = alTrackedBodies[bodyIndex].position;
|
|
|
|
float userDist = 0f;
|
|
if (userDetectionOrder == KinectManager.UserDetectionOrder.Distance)
|
|
userDist = Mathf.Abs(userPos.x) + Mathf.Abs(userPos.z);
|
|
else if (userDetectionOrder == KinectManager.UserDetectionOrder.LeftToRight)
|
|
userDist = userPos.x;
|
|
|
|
for (int i = 0; i < aUserIndexIds.Length; i++)
|
|
{
|
|
if (aUserIndexIds[i] == 0)
|
|
{
|
|
// free user slot
|
|
uidIndex = i;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
ulong uidUserId = aUserIndexIds[i];
|
|
Vector3 uidUserPos = kinectManager.GetUserPosition(uidUserId);
|
|
|
|
float uidUserDist = 0;
|
|
if (userDetectionOrder == KinectManager.UserDetectionOrder.Distance)
|
|
uidUserDist = Mathf.Abs(uidUserPos.x) + Mathf.Abs(uidUserPos.z);
|
|
else if (userDetectionOrder == KinectManager.UserDetectionOrder.LeftToRight)
|
|
uidUserDist = uidUserPos.x;
|
|
|
|
if (userDist <= uidUserDist)
|
|
{
|
|
// current user is left to the compared one
|
|
for (int u = (aUserIndexIds.Length - 2); u >= i; u--)
|
|
{
|
|
aUserIndexIds[u + 1] = aUserIndexIds[u];
|
|
|
|
if (aUserIndexIds[u] != 0)
|
|
{
|
|
Debug.Log(string.Format("Reindexing user {0} to {1}, ID: {2}.", u, u + 1, aUserIndexIds[u]));
|
|
}
|
|
}
|
|
|
|
aUserIndexIds[i] = 0; // cleanup current index
|
|
uidIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
// look for the 1st available slot
|
|
for (int i = 0; i < aUserIndexIds.Length; i++)
|
|
{
|
|
if (aUserIndexIds[i] == 0)
|
|
{
|
|
uidIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return uidIndex;
|
|
}
|
|
|
|
|
|
// releases the user slot. rearranges the remaining users.
|
|
protected virtual void FreeEmptyUserSlot(int uidIndex, KinectManager.UserDetectionOrder userDetectionOrder)
|
|
{
|
|
aUserIndexIds[uidIndex] = 0;
|
|
|
|
if (userDetectionOrder != KinectManager.UserDetectionOrder.Appearance)
|
|
{
|
|
// rearrange the remaining users
|
|
for (int u = uidIndex; u < (aUserIndexIds.Length - 1); u++)
|
|
{
|
|
aUserIndexIds[u] = aUserIndexIds[u + 1];
|
|
|
|
if (aUserIndexIds[u + 1] != 0)
|
|
{
|
|
Debug.Log(string.Format("Reindexing user {0} to {1}, ID: {2}.", u + 1, u, aUserIndexIds[u + 1]));
|
|
}
|
|
}
|
|
|
|
// make sure the last slot is free
|
|
aUserIndexIds[aUserIndexIds.Length - 1] = 0;
|
|
}
|
|
|
|
// rearrange the remaining users
|
|
RearrangeUserIndices(userDetectionOrder);
|
|
}
|
|
|
|
|
|
// Adds UserId to the list of users
|
|
public virtual int CalibrateUser(ulong userId, int bodyIndex, ref KinectInterop.BodyData[] alTrackedBodies,
|
|
KinectManager.UserDetectionOrder userDetectionOrder, GestureType playerCalibrationPose, KinectGestureManager gestureManager)
|
|
{
|
|
if (!alUserIds.Contains(userId))
|
|
{
|
|
if (CheckForCalibrationPose(userId, bodyIndex, playerCalibrationPose, gestureManager, ref alTrackedBodies))
|
|
{
|
|
//int uidIndex = alUserIds.Count;
|
|
int uidIndex = GetEmptyUserSlot(userId, bodyIndex, ref alTrackedBodies, userDetectionOrder);
|
|
|
|
if (uidIndex >= 0)
|
|
{
|
|
aUserIndexIds[uidIndex] = userId;
|
|
}
|
|
else
|
|
{
|
|
// no empty user-index slot
|
|
return -1;
|
|
}
|
|
|
|
dictUserIdToIndex[userId] = bodyIndex;
|
|
dictUserIdToTime[userId] = Time.time;
|
|
alUserIds.Add(userId);
|
|
|
|
// set primary user-id, if there is none
|
|
if (liPrimaryUserId == 0 && aUserIndexIds.Length > 0)
|
|
{
|
|
liPrimaryUserId = aUserIndexIds[0]; // userId
|
|
}
|
|
|
|
return uidIndex;
|
|
}
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
|
|
// fires the OnUserAdded-event
|
|
internal void FireOnUserAdded(ulong userId, int userIndex)
|
|
{
|
|
OnUserAdded?.Invoke(userId, userIndex);
|
|
}
|
|
|
|
|
|
// Remove a lost UserId
|
|
public virtual int RemoveUser(ulong userId, KinectManager.UserDetectionOrder userDetectionOrder)
|
|
{
|
|
//int uidIndex = alUserIds.IndexOf(userId);
|
|
int uidIndex = System.Array.IndexOf(aUserIndexIds, userId);
|
|
|
|
// clear calibration data for this user
|
|
if (playerCalibrationData.ContainsKey(userId))
|
|
{
|
|
playerCalibrationData.Remove(userId);
|
|
}
|
|
|
|
// clean up the outdated calibration data in the data dictionary
|
|
List<ulong> alCalDataKeys = new List<ulong>(playerCalibrationData.Keys);
|
|
|
|
foreach (ulong calUserID in alCalDataKeys)
|
|
{
|
|
KinectGestureManager.GestureData gestureData = playerCalibrationData[calUserID];
|
|
|
|
if ((gestureData.timestamp + 60f) < Time.realtimeSinceStartup)
|
|
{
|
|
playerCalibrationData.Remove(calUserID);
|
|
}
|
|
}
|
|
|
|
alCalDataKeys.Clear();
|
|
|
|
// remove user-id from the global users lists
|
|
dictUserIdToIndex.Remove(userId);
|
|
dictUserIdToTime.Remove(userId);
|
|
alUserIds.Remove(userId);
|
|
|
|
if (uidIndex >= 0)
|
|
{
|
|
FreeEmptyUserSlot(uidIndex, userDetectionOrder);
|
|
}
|
|
|
|
// if this was the primary user, update the primary user-id
|
|
if (liPrimaryUserId == userId)
|
|
{
|
|
if (aUserIndexIds.Length > 0)
|
|
{
|
|
liPrimaryUserId = aUserIndexIds[0];
|
|
}
|
|
else
|
|
{
|
|
liPrimaryUserId = 0;
|
|
}
|
|
}
|
|
|
|
if (alUserIds.Count == 0)
|
|
{
|
|
//Debug.Log("Waiting for users.");
|
|
}
|
|
|
|
return uidIndex;
|
|
}
|
|
|
|
|
|
// fires the OnUserRemoved-event
|
|
internal void FireOnUserRemoved(ulong userId, int userIndex)
|
|
{
|
|
OnUserRemoved?.Invoke(userId, userIndex);
|
|
}
|
|
|
|
|
|
// check if the calibration pose is complete for given user
|
|
protected virtual bool CheckForCalibrationPose(ulong UserId, int bodyIndex, GestureType calibrationGesture,
|
|
KinectGestureManager gestureManager, ref KinectInterop.BodyData[] alTrackedBodies)
|
|
{
|
|
if (calibrationGesture == GestureType.None)
|
|
return true;
|
|
if (!gestureManager)
|
|
return false;
|
|
|
|
KinectGestureManager.GestureData gestureData = playerCalibrationData.ContainsKey(UserId) ?
|
|
playerCalibrationData[UserId] : new KinectGestureManager.GestureData();
|
|
|
|
// init gesture data if needed
|
|
if (gestureData.userId != UserId)
|
|
{
|
|
gestureData.userId = UserId;
|
|
gestureData.gesture = calibrationGesture;
|
|
gestureData.state = 0;
|
|
gestureData.timestamp = Time.realtimeSinceStartup;
|
|
gestureData.joint = 0;
|
|
gestureData.progress = 0f;
|
|
gestureData.complete = false;
|
|
gestureData.cancelled = false;
|
|
}
|
|
|
|
// get joint positions and tracking
|
|
int iAllJointsCount = (int)KinectInterop.JointType.Count;
|
|
bool[] playerJointsTracked = new bool[iAllJointsCount];
|
|
Vector3[] playerJointsPos = new Vector3[iAllJointsCount];
|
|
|
|
int[] aiNeededJointIndexes = gestureManager.GetNeededJointIndexes();
|
|
int iNeededJointsCount = aiNeededJointIndexes.Length;
|
|
|
|
for (int i = 0; i < iNeededJointsCount; i++)
|
|
{
|
|
int joint = aiNeededJointIndexes[i];
|
|
|
|
if (joint >= 0)
|
|
{
|
|
KinectInterop.JointData jointData = alTrackedBodies[bodyIndex].joint[joint];
|
|
|
|
playerJointsTracked[joint] = jointData.trackingState != KinectInterop.TrackingState.NotTracked;
|
|
playerJointsPos[joint] = jointData.kinectPos;
|
|
|
|
if (!playerJointsTracked[joint] && (joint == (int)KinectInterop.JointType.Neck))
|
|
{
|
|
KinectInterop.JointData lShoulderData = alTrackedBodies[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderLeft];
|
|
KinectInterop.JointData rShoulderData = alTrackedBodies[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderRight];
|
|
|
|
if (lShoulderData.trackingState != KinectInterop.TrackingState.NotTracked && rShoulderData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
playerJointsTracked[joint] = true;
|
|
playerJointsPos[joint] = (lShoulderData.kinectPos + rShoulderData.kinectPos) / 2f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// estimate the gesture progess
|
|
gestureManager.CheckForGesture(UserId, ref gestureData, Time.realtimeSinceStartup,
|
|
ref playerJointsPos, ref playerJointsTracked);
|
|
playerCalibrationData[UserId] = gestureData;
|
|
|
|
// check if gesture is complete
|
|
if (gestureData.complete)
|
|
{
|
|
gestureData.userId = 0;
|
|
playerCalibrationData[UserId] = gestureData;
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
}
|
|
}
|
|
|