// CopySkeleton copies the data from another skeleton.
public static void CopySkeleton(ref KinectWrapper.NuiSkeletonData source, ref KinectWrapper.NuiSkeletonData destination)
{
// if (null == source)
// {
// return;
// }
//
// if (null == destination)
// {
// destination = new Skeleton();
// }
destination.eTrackingState = source.eTrackingState;
destination.dwTrackingID = source.dwTrackingID;
destination.Position = source.Position;
destination.dwQualityFlags = source.dwQualityFlags;
int jointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count;
if (destination.SkeletonPositions == null)
{
destination.SkeletonPositions = new Vector4[jointsCount];
}
if (destination.eSkeletonPositionTrackingState == null)
{
destination.eSkeletonPositionTrackingState = new KinectWrapper.NuiSkeletonPositionTrackingState[jointsCount];
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
destination.SkeletonPositions[jointIndex] = source.SkeletonPositions[jointIndex];
destination.eSkeletonPositionTrackingState[jointIndex] = source.eSkeletonPositionTrackingState[jointIndex];
}
}
// Assign UserId to player 1 or 2.
void CalibrateUser(uint UserId, int UserIndex, ref KinectWrapper.NuiSkeletonData skeletonData)
{
// If player 1 hasn't been calibrated, assign that UserID to it.
if (!Player1Calibrated)
{
// Check to make sure we don't accidentally assign player 2 to player 1.
if (!allUsers.Contains(UserId))
{
Player1Calibrated = true;
Player1ID = UserId;
Player1Index = UserIndex;
allUsers.Add(UserId);
// reset skeleton filters
ResetFilters();
AllPlayersCalibrated = allUsers.Count >= 1;
}
}
// If all users are calibrated, stop trying to find them.
if (AllPlayersCalibrated)
{
Debug.Log("All players calibrated.");
}
}
// ConstrainSelfIntersection collides joints with the skeleton to keep the skeleton's hands and wrists from puncturing its body
// A cylinder is created to represent the torso. Intersecting joints have their positions changed to push them outside the torso.
public void Constrain(ref KinectWrapper.NuiSkeletonData skeleton)
{
// if (null == skeleton)
// {
// return;
// }
int shoulderCenterIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderCenter;
int hipCenterIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter;
if (skeleton.eSkeletonPositionTrackingState[shoulderCenterIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked &&
skeleton.eSkeletonPositionTrackingState[hipCenterIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked)
{
Vector3 shoulderDiffLeft = KinectHelper.VectorBetween(ref skeleton, shoulderCenterIndex, (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderLeft);
Vector3 shoulderDiffRight = KinectHelper.VectorBetween(ref skeleton, shoulderCenterIndex, (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderRight);
float shoulderLengthLeft = shoulderDiffLeft.magnitude;
float shoulderLengthRight = shoulderDiffRight.magnitude;
// The distance between shoulders is averaged for the radius
float cylinderRadius = (shoulderLengthLeft + shoulderLengthRight) * 0.5f;
// Calculate the shoulder center and the hip center. Extend them up and down respectively.
Vector3 shoulderCenter = (Vector3)skeleton.SkeletonPositions[shoulderCenterIndex];
Vector3 hipCenter = (Vector3)skeleton.SkeletonPositions[hipCenterIndex];
Vector3 hipShoulder = hipCenter - shoulderCenter;
hipShoulder.Normalize();
shoulderCenter = shoulderCenter - (hipShoulder * (ShoulderExtend * cylinderRadius));
hipCenter = hipCenter + (hipShoulder * (HipExtend * cylinderRadius));
// Optionally increase radius to account for bulky avatars
cylinderRadius *= RadiusMultiplier;
// joints to collide
int[] collisionIndices =
{
(int)KinectWrapper.NuiSkeletonPositionIndex.WristLeft,
(int)KinectWrapper.NuiSkeletonPositionIndex.HandLeft,
(int)KinectWrapper.NuiSkeletonPositionIndex.WristRight,
(int)KinectWrapper.NuiSkeletonPositionIndex.HandRight
};
foreach (int j in collisionIndices)
{
Vector3 collisionJoint = (Vector3)skeleton.SkeletonPositions[j];
Vector4 distanceNormal = KinectHelper.DistanceToLineSegment(shoulderCenter, hipCenter, collisionJoint);
Vector3 normal = new Vector3(distanceNormal.x, distanceNormal.y, distanceNormal.z);
// if distance is within the cylinder then push the joint out and away from the cylinder
if (distanceNormal.w < cylinderRadius)
{
collisionJoint += normal * ((cylinderRadius - distanceNormal.w) * CollisionTolerance);
skeleton.SkeletonPositions[j] = (Vector4)collisionJoint;
}
}
}
}
// IsTrackedOrInferred checks whether the skeleton joint is tracked or inferred.
public static bool IsTrackedOrInferred(KinectWrapper.NuiSkeletonData skeleton, int jointIndex)
{
// if (null == skeleton)
// {
// return false;
// }
return(skeleton.eSkeletonPositionTrackingState[jointIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked);
}
// Process the skeleton data
void ProcessSkeleton()
{
List <uint> lostUsers = new List <uint>();
lostUsers.AddRange(allUsers);
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
uint userId = skeletonData.dwTrackingID;
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
if (!allUsers.Contains(userId))
{
allUsers.Add(userId);
}
uint[] user = allUsers.ToArray();
if (userId == user[0])
{
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
bool playerTracked = IgnoreInferredJoints ?
(int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(Array.BinarySearch(mustBeTrackedJoints, j) >= 0 ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked);
playerJointsTracked[j] = playerPrevTracked[j] && playerTracked;
playerPrevTracked[j] = playerTracked;
if (playerJointsTracked[j])
{
playerJointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
}
}
// draw the skeleton on top of texture
if (ComputeUserMap)
{
usersLblTex.Apply();
}
}
lostUsers.Remove(userId);
}
}
// remove the lost users if any
if (lostUsers.Count > 0)
{
foreach (uint userId in lostUsers)
{
RemoveUser(userId);
}
lostUsers.Clear();
}
}
/// VectorBetween calculates the Vector3 from start to end == subtract start from end
public static Vector3 VectorBetween(ref KinectWrapper.NuiSkeletonData skeleton, int startJoint, int endJoint)
{
// if (null == skeleton)
// {
// return Vector3.Zero;
// }
Vector3 startPosition = (Vector3)skeleton.SkeletonPositions[startJoint];
Vector3 endPosition = (Vector3)skeleton.SkeletonPositions[endJoint];
return(endPosition - startPosition);
}
// LerpAndApply performs a Lerp and applies the Lerped vector to the skeleton joint.
public static void LerpAndApply(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, Vector3 newJointPos, float lerpValue, KinectWrapper.NuiSkeletonPositionTrackingState finalTrackingState)
{
// if (null == skeleton)
// {
// return;
// }
Vector3 jointPos = (Vector3)skeleton.SkeletonPositions[jointIndex];
jointPos = Vector3.Lerp(jointPos, newJointPos, lerpValue);
skeleton.SkeletonPositions[jointIndex] = (Vector4)jointPos;
skeleton.eSkeletonPositionTrackingState[jointIndex] = finalTrackingState;
}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectWrapper.NuiSkeletonData skeleton)
{
// if (null == skeleton)
// {
// return;
// }
if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
return;
}
if (this.init == false)
{
this.Init(); // initialize with default parameters
}
//Array jointTypeValues = Enum.GetValues(typeof(KinectWrapper.NuiSkeletonPositionIndex));
KinectWrapper.NuiTransformSmoothParameters tempSmoothingParams = new KinectWrapper.NuiTransformSmoothParameters();
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
this.smoothParameters.fJitterRadius = Math.Max(0.0001f, this.smoothParameters.fJitterRadius);
tempSmoothingParams.fSmoothing = smoothParameters.fSmoothing;
tempSmoothingParams.fCorrection = smoothParameters.fCorrection;
tempSmoothingParams.fPrediction = smoothParameters.fPrediction;
int jointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count;
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
//KinectWrapper.NuiSkeletonPositionIndex jt = (KinectWrapper.NuiSkeletonPositionIndex)jointTypeValues.GetValue(jointIndex);
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
if (skeleton.eSkeletonPositionTrackingState[jointIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.Tracked)
{
tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius * 2.0f;
tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius;
tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius;
}
FilterJoint(ref skeleton, jointIndex, ref tempSmoothingParams);
}
}
// Assign UserId to player 1 or 2.
void CalibrateUser(uint UserId, int UserIndex, ref KinectWrapper.NuiSkeletonData skeletonData)
{
// If player 1 hasn't been calibrated, assign that UserID to it.
if (!_player1Calibrated)
{
// Check to make sure we don't accidentally assign player 2 to player 1.
if (!_allUsers.Contains(UserId))
{
_player1Calibrated = true;
_player1Id = UserId;
_player1Index = UserIndex;
_allUsers.Add(UserId);
// reset skeleton filters
ResetFilters();
// If we're not using 2 users, we're all calibrated.
//if(!TwoUsers)
{
_allPlayersCalibrated = !TwoUsers ? _allUsers.Count >= 1 : _allUsers.Count >= 2; // true;
}
}
}
// Otherwise, assign to player 2.
else if (TwoUsers && !_player2Calibrated)
{
if (!_allUsers.Contains(UserId))
{
_player2Calibrated = true;
_player2Id = UserId;
_player2Index = UserIndex;
_allUsers.Add(UserId);
// reset skeleton filters
ResetFilters();
// All users are calibrated!
_allPlayersCalibrated = !TwoUsers ? _allUsers.Count >= 1 : _allUsers.Count >= 2; // true;
}
}
// If all users are calibrated, stop trying to find them.
if (_allPlayersCalibrated)
{
Debug.Log("All players calibrated.");
}
}
// Initializes a new instance of the class.
public ClippedLegsFilter()
{
this.lerpLeftKnee = new TimedLerp();
this.lerpLeftAnkle = new TimedLerp();
this.lerpLeftFoot = new TimedLerp();
this.lerpRightKnee = new TimedLerp();
this.lerpRightAnkle = new TimedLerp();
this.lerpRightFoot = new TimedLerp();
this.filterJoints = new JointPositionsFilter();
this.filteredSkeleton = new KinectWrapper.NuiSkeletonData();
// knee, ankle, foot blend amounts
this.allTracked = new Vector3(0.0f, 0.0f, 0.0f); // All joints are tracked
this.footInferred = new Vector3(0.0f, 0.0f, 1.0f); // foot is inferred
this.ankleInferred = new Vector3(0.5f, 1.0f, 1.0f); // ankle is inferred
this.kneeInferred = new Vector3(1.0f, 1.0f, 1.0f); // knee is inferred
Reset();
}
// Initializes a new instance of the class.
public ClippedLegsFilter()
{
this.lerpLeftKnee = new TimedLerp();
this.lerpLeftAnkle = new TimedLerp();
this.lerpLeftFoot = new TimedLerp();
this.lerpRightKnee = new TimedLerp();
this.lerpRightAnkle = new TimedLerp();
this.lerpRightFoot = new TimedLerp();
this.filterJoints = new JointPositionsFilter();
this.filteredSkeleton = new KinectWrapper.NuiSkeletonData();
// knee, ankle, foot blend amounts
this.allTracked = new Vector3(0.0f, 0.0f, 0.0f); // All joints are tracked
this.footInferred = new Vector3(0.0f, 0.0f, 1.0f); // foot is inferred
this.ankleInferred = new Vector3(0.5f, 1.0f, 1.0f); // ankle is inferred
this.kneeInferred = new Vector3(1.0f, 1.0f, 1.0f); // knee is inferred
Reset();
}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectWrapper.NuiSkeletonData skeleton)
{
if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
return;
}
if (this.init == false)
{
this.Init(); // initialize with default parameters
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
smoothParameters.fJitterRadius = Math.Max(0.0001f, smoothParameters.fJitterRadius);
int jointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count;
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
FilterJoint(ref skeleton, jointIndex, ref smoothParameters);
}
}
// Update the filter for one joint.
protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters)
{
// if (null == skeleton)
// {
// return;
// }
//int jointIndex = (int)jt;
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
Vector3 predictedPosition; // modify
float diffVal;
Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex];
Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
Vector3 prevTrend = this.history[jointIndex].Trend;
Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
Vector3 prevPredictedPosition = this.history[jointIndex].PredictedPosition; // modify
bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition);
// If joint is invalid, reset the filter
if (!jointIsValid)
{
history[jointIndex].FrameCount = 0;
}
// modify
if (filterType == 1)
{
// Initial start values
// Single Exponential Smoothing Filter
if (this.history[jointIndex].FrameCount == 0)
{
filteredPosition = rawPosition;
}
else if (this.history[jointIndex].FrameCount == 1 || this.history[jointIndex].FrameCount == 2)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
}
else
{
// First apply jitter filter
diffvec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal <= smoothingParameters.fJitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Single exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + (prevFilteredPosition * smoothingParameters.fSmoothing);
}
predictedPosition = filteredPosition;
// Check if we are not too far away from raw data
diffvec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffvec.magnitude);
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
trend = Vector3.zero;
}
else
{
// Initial start values
// Double Exponential Smoothing Filter
if (this.history[jointIndex].FrameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (this.history[jointIndex].FrameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffvec = filteredPosition - prevFilteredPosition;
trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
else
{
// First apply jitter filter
diffvec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal <= smoothingParameters.fJitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.fSmoothing);
diffvec = filteredPosition - prevFilteredPosition;
trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
// Predict into the future to reduce latency
// Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction);
predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction); //modify
// Check if we are not too far away from raw data
diffvec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffvec.magnitude);
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
}
// Save the data from this frame
history[jointIndex].RawPosition = rawPosition;
history[jointIndex].FilteredPosition = filteredPosition;
history[jointIndex].Trend = trend;
history[jointIndex].FrameCount++;
// Set the filtered data back into the joint
// Joint j = skeleton.Joints[jt];
// j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition);
// skeleton.Joints[jt] = j;
skeleton.SkeletonPositions[jointIndex] = (Vector4)predictedPosition;
}
// Implements the per-frame filter logic for the arms up patch.
public bool FilterSkeleton(ref KinectWrapper.NuiSkeletonData skeleton, float deltaNuiTime)
{
// if (null == skeleton)
// {
// return false;
// }
// exit early if we lose tracking on the entire skeleton
if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
filterJoints.Reset();
}
KinectHelper.CopySkeleton(ref skeleton, ref filteredSkeleton);
filterJoints.UpdateFilter(ref filteredSkeleton);
// Update lerp state with the current delta NUI time.
this.lerpLeftKnee.Tick(deltaNuiTime);
this.lerpLeftAnkle.Tick(deltaNuiTime);
this.lerpLeftFoot.Tick(deltaNuiTime);
this.lerpRightKnee.Tick(deltaNuiTime);
this.lerpRightAnkle.Tick(deltaNuiTime);
this.lerpRightFoot.Tick(deltaNuiTime);
// Exit early if we do not have a valid body basis - too much of the skeleton is invalid.
if ((!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter)) ||
(!KinectHelper.IsTrackedOrInferred(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipLeft)) ||
(!KinectHelper.IsTrackedOrInferred(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipRight)))
{
return(false);
}
// Determine if the skeleton is clipped by the bottom of the FOV.
bool clippedBottom = (skeleton.dwQualityFlags & (int)KinectWrapper.FrameEdges.Bottom) != 0;
// Select a mask for the left leg depending on which joints are not tracked.
// These masks define how much of the filtered joint positions should be blended
// with the raw positions. Based on the tracking state of the leg joints, we apply
// more filtered data as more joints lose tracking.
Vector3 leftLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.KneeLeft))
{
leftLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft))
{
leftLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft))
{
leftLegMask = this.footInferred;
}
// Select a mask for the right leg depending on which joints are not tracked.
Vector3 rightLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.KneeRight))
{
rightLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight))
{
rightLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.FootRight))
{
rightLegMask = this.footInferred;
}
// If the skeleton is not clipped by the bottom of the FOV, cut the filtered data
// blend in half.
float clipMask = clippedBottom ? 1.0f : 0.5f;
// Apply the mask values to the joints of each leg, by placing the mask values into the lerp targets.
this.lerpLeftKnee.SetEnabled(leftLegMask.x * clipMask);
this.lerpLeftAnkle.SetEnabled(leftLegMask.y * clipMask);
this.lerpLeftFoot.SetEnabled(leftLegMask.z * clipMask);
this.lerpRightKnee.SetEnabled(rightLegMask.x * clipMask);
this.lerpRightAnkle.SetEnabled(rightLegMask.y * clipMask);
this.lerpRightFoot.SetEnabled(rightLegMask.z * clipMask);
// The bSkeletonUpdated flag tracks whether we have modified the output skeleton or not.
bool skeletonUpdated = false;
// Apply lerp to the left knee, which will blend the raw joint position with the filtered joint position based on the current lerp value.
if (this.lerpLeftKnee.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.KneeLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftKnee.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left ankle.
if (this.lerpLeftAnkle.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftAnkle.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left foot.
if (this.lerpLeftFoot.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftFoot.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Inferred);
skeletonUpdated = true;
}
// Apply lerp to the right knee.
if (this.lerpRightKnee.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.KneeRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightKnee.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right ankle.
if (this.lerpRightAnkle.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightAnkle.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right foot.
if (this.lerpRightFoot.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.FootRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightFoot.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Inferred);
skeletonUpdated = true;
}
return(skeletonUpdated);
}
// Process the skeleton data
void ProcessSkeleton()
{
List <uint> lostUsers = new List <uint>();
lostUsers.AddRange(_allUsers);
// calculate the time since last update
float currentNuiTime = Time.realtimeSinceStartup;
float deltaNuiTime = currentNuiTime - _lastNuiTime;
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = _skeletonFrame.SkeletonData[i];
uint userId = skeletonData.dwTrackingID;
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
// get the skeleton position
Vector3 skeletonPos = _kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
if (!_allPlayersCalibrated)
{
// check if this is the closest user
bool bClosestUser = true;
if (DetectClosestUser)
{
for (int j = 0; j < KinectWrapper.Constants.NuiSkeletonCount; j++)
{
if (j != i)
{
KinectWrapper.NuiSkeletonData skeletonDataOther = _skeletonFrame.SkeletonData[j];
if ((skeletonDataOther.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked) &&
(Mathf.Abs(_kinectToWorld.MultiplyPoint3x4(skeletonDataOther.Position).z) < Mathf.Abs(skeletonPos.z)))
{
bClosestUser = false;
break;
}
}
}
}
if (bClosestUser)
{
CalibrateUser(userId, i + 1, ref skeletonData);
}
}
if (userId == _player1Id && Mathf.Abs(skeletonPos.z) >= MinUserDistance &&
(MaxUserDistance <= 0f || Mathf.Abs(skeletonPos.z) <= MaxUserDistance))
{
_player1SkeletonIndex = i;
// get player position
_player1Pos = skeletonPos;
// get joints' position and rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
bool playerTracked = IgnoreInferredJoints ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(Array.BinarySearch(mustBeTrackedJoints, j) >= 0 ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked);
_player1JointsTracked[j] = _player1PrevTracked[j] && playerTracked;
_player1PrevTracked[j] = playerTracked;
if (_player1JointsTracked[j])
{
_player1JointsPos[j] = _kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
}
}
// calculate joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref _player1JointsPos, ref _player1JointsTracked, ref _player1JointsOri);
// get player rotation
_player1Ori = _player1JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
else if (userId == _player2Id && Mathf.Abs(skeletonPos.z) >= MinUserDistance &&
(MaxUserDistance <= 0f || Mathf.Abs(skeletonPos.z) <= MaxUserDistance))
{
_player2SkeletonIndex = i;
// get player position
_player2Pos = skeletonPos;
// get joints' position and rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
bool playerTracked = IgnoreInferredJoints ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(Array.BinarySearch(mustBeTrackedJoints, j) >= 0 ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked);
_player2JointsTracked[j] = _player2PrevTracked[j] && playerTracked;
_player2PrevTracked[j] = playerTracked;
if (_player2JointsTracked[j])
{
_player2JointsPos[j] = _kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
}
}
// calculate joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref _player2JointsPos, ref _player2JointsTracked, ref _player2JointsOri);
// get player rotation
_player2Ori = _player2JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
lostUsers.Remove(userId);
}
}
// update the nui-timer
_lastNuiTime = currentNuiTime;
// remove the lost users if any
if (lostUsers.Count > 0)
{
foreach (uint userId in lostUsers)
{
RemoveUser(userId);
}
lostUsers.Clear();
}
}
// Update the filter for one joint.
protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters, ref Matrix4x4[] jointOrientations)
{
// if (null == skeleton)
// {
// return;
// }
// int jointIndex = (int)jt;
Quaternion filteredOrientation;
Quaternion trend;
Vector3 fwdVector = (Vector3)jointOrientations[jointIndex].GetColumn(2);
if (fwdVector == Vector3.zero)
{
return;
}
Quaternion rawOrientation = Quaternion.LookRotation(fwdVector, jointOrientations[jointIndex].GetColumn(1));
Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation;
Quaternion prevTrend = this.history[jointIndex].Trend;
Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex];
bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jointIndex) && KinectHelper.BoneOrientationIsValid(rawOrientation);
if (!orientationIsValid)
{
if (this.history[jointIndex].FrameCount > 0)
{
rawOrientation = history[jointIndex].FilteredBoneOrientation;
history[jointIndex].FrameCount = 0;
}
}
// Initial start values or reset values
if (this.history[jointIndex].FrameCount == 0)
{
// Use raw position and zero trend for first value
filteredOrientation = rawOrientation;
trend = Quaternion.identity;
}
else if (this.history[jointIndex].FrameCount == 1)
{
// Use average of two positions and calculate proper trend for end value
Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation;
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f);
Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.fCorrection);
}
else
{
// First apply a jitter filter
Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation);
float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter));
if (diffValJitter <= smoothingParameters.fJitterRadius)
{
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.fJitterRadius);
}
else
{
filteredOrientation = rawOrientation;
}
// Now the double exponential smoothing filter
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, prevFilteredOrientation * prevTrend, smoothingParameters.fSmoothing);
diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.fCorrection);
}
// Use the trend and predict into the future to reduce latency
Quaternion predictedOrientation = filteredOrientation * KinectHelper.EnhancedQuaternionSlerp(Quaternion.identity, trend, smoothingParameters.fPrediction);
// Check that we are not too far away from raw data
Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation);
float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff));
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.fMaxDeviationRadius / diffVal);
}
// predictedOrientation.Normalize();
// filteredOrientation.Normalize();
// trend.Normalize();
// Save the data from this frame
history[jointIndex].RawBoneOrientation = rawOrientation;
history[jointIndex].FilteredBoneOrientation = filteredOrientation;
history[jointIndex].Trend = trend;
history[jointIndex].FrameCount++;
// Set the filtered and predicted data back into the bone orientation
if (KinectHelper.BoneOrientationIsValid(predictedOrientation))
{
jointOrientations[jointIndex].SetTRS(Vector3.zero, predictedOrientation, Vector3.one);
}
}
// Update the filter for one joint.
protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters)
{
float filteredState;
float trend;
float diffVal;
float rawState = (float)skeleton.eSkeletonPositionTrackingState[jointIndex];
float prevFilteredState = history[jointIndex].FilteredState;
float prevTrend = history[jointIndex].Trend;
float prevRawState = history[jointIndex].RawState;
// If joint is invalid, reset the filter
if (rawState == 0f)
{
history[jointIndex].FrameCount = 0;
}
// Initial start values
if (history[jointIndex].FrameCount == 0)
{
filteredState = rawState;
trend = 0f;
}
else if (this.history[jointIndex].FrameCount == 1)
{
filteredState = (rawState + prevRawState) * 0.5f;
diffVal = filteredState - prevFilteredState;
trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
else
{
// // First apply jitter filter
// diffVal = rawState - prevFilteredState;
//
// if (diffVal <= smoothingParameters.fJitterRadius)
// {
// filteredState = (rawState * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredState * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
// }
// else
// {
// filteredState = rawState;
// }
filteredState = rawState;
// Now the double exponential smoothing filter
filteredState = (filteredState * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredState + prevTrend) * smoothingParameters.fSmoothing);
diffVal = filteredState - prevFilteredState;
trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
// Predict into the future to reduce latency
float predictedState = filteredState + (trend * smoothingParameters.fPrediction);
// Check that we are not too far away from raw data
diffVal = predictedState - rawState;
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedState = (predictedState * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawState * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
// Save the data from this frame
history[jointIndex].RawState = rawState;
history[jointIndex].FilteredState = filteredState;
history[jointIndex].Trend = trend;
history[jointIndex].FrameCount++;
// Set the filtered data back into the joint
skeleton.eSkeletonPositionTrackingState[jointIndex] = (KinectWrapper.NuiSkeletonPositionTrackingState)(predictedState + 0.5f);
}
/// <summary>
/// Process the skeleton data
/// </summary>
void ProcessSkeleton()
{
List <KinectWrapper.NuiSkeletonData> allTrackedSkeletons = new List <KinectWrapper.NuiSkeletonData>();
// Look for skeletons, that are tracked now and on the previous frame
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked && lastFrameTrackedSkeletonDWTrackingId.Contains(skeletonData.dwTrackingID))
{
allTrackedSkeletons.Add(skeletonData);
}
}
// Now update lastFrameTrackedSkeletonDWTrackingId
lastFrameTrackedSkeletonDWTrackingId.RemoveRange(0, lastFrameTrackedSkeletonDWTrackingId.Count);
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
lastFrameTrackedSkeletonDWTrackingId.Add(skeletonData.dwTrackingID);
}
}
// Now work with skeletons that are fully tracked at least two frames long
if (allTrackedSkeletons.Count < 1)
{
// if skeleton was tracked last frame and is not tracked now then ...
// public variable
SkeletonIsTracked = false;
RightHandPosition = new Vector3(-10f, -10f, -10f);
LeftHandPosition = new Vector3(-10f, -10f, -10f);
HipCenterPosition = new Vector3(-10f, -10f, -10f);
if (KinectText != null)
{
KinectText.text = "No players ..."; //"Не бачу гравця ...";
}
return;
}
// if skeleton is tracked now but was not tracked last frame then ...
if (SkeletonIsTracked == false)
{
SkeletonIsTracked = true;
}
// Choose the closest tracked skeleton
KinectWrapper.NuiSkeletonData activeSkeletonData;
if (allTrackedSkeletons.Count == 1)
{
activeSkeletonData = allTrackedSkeletons[0];
if (KinectText != null)
{
KinectText.text = "I see one player.";//"Бачу гравця: ";//+ activeSkeletonData.dwTrackingID;
}
}
else
{
// sort skeletons by Z position and then choose the closest
for (int i = 0; i < allTrackedSkeletons.Count - 1; i++)
{
for (int j = i + 1; j < allTrackedSkeletons.Count; j++)
{
if (allTrackedSkeletons[i].Position.z > allTrackedSkeletons[j].Position.z)
{
KinectWrapper.NuiSkeletonData temp = allTrackedSkeletons[i];
allTrackedSkeletons[i] = allTrackedSkeletons[j];
allTrackedSkeletons[j] = allTrackedSkeletons[i];
}
}
}
activeSkeletonData = allTrackedSkeletons[0];
if (KinectText != null)
{
KinectText.text = "I see too many players.";//"Надто багато гравців.";// + allTrackedSkeletons.Count.ToString();
}
}
// Now activeSkeletonData is active tracked skeleton
HipCenterPosition = new Vector3(activeSkeletonData.SkeletonPositions[0].x, activeSkeletonData.SkeletonPositions[0].y, activeSkeletonData.SkeletonPositions[0].z); // 0 == HipCenter
RightHandPosition = new Vector3(activeSkeletonData.SkeletonPositions[11].x, activeSkeletonData.SkeletonPositions[11].y, activeSkeletonData.SkeletonPositions[11].z); // 11 == HandRight
LeftHandPosition = new Vector3(activeSkeletonData.SkeletonPositions[7].x, activeSkeletonData.SkeletonPositions[7].y, activeSkeletonData.SkeletonPositions[7].z); // 7 == HandLeft
return;
}
// Process the skeleton data
void ProcessSkeleton()
{
List <uint> lostUsers = new List <uint>();
lostUsers.AddRange(allUsers);
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
uint userId = skeletonData.dwTrackingID;
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
if (!AllPlayersCalibrated)
{
CalibrateUser(userId);
}
//int stateNotTracked = (int)KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked; // Waterstrong
int stateTracked = (int)KinectWrapper.NuiSkeletonPositionTrackingState.Tracked;
if (userId == Player1ID)
{
// get player position
player1Pos = kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
player1Pos.z = -player1Pos.z;
//Debug.Log(String.Format("({0}, {1}, {2})", player1Pos.x, player1Pos.y, player1Pos.z));
//CalibrationText.guiText.text = String.Format("({0:F1}, {1:F1}, {2:F1})", player1Pos.x, player1Pos.y, player1Pos.z);
// get joints rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
//player1JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked; // Waterstrong
player1JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked;
if (player1JointsTracked[j])
{
player1JointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
player1JointsPos[j].z = -player1JointsPos[j].z;
}
}
// get joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref player1JointsPos, ref player1JointsTracked, ref player1JointsOri);
// get player rotation
player1Ori = player1JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
else if (userId == Player2ID)
{
// get player position
player2Pos = kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
player2Pos.z = -player2Pos.z;
// get joints rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
//player2JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked; // Waterstrong
player2JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked;
if (player2JointsTracked[j])
{
player2JointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
player2JointsPos[j].z = -player2JointsPos[j].z;
}
}
// get joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref player2JointsPos, ref player2JointsTracked, ref player2JointsOri);
// get player rotation
player2Ori = player2JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
lostUsers.Remove(userId);
}
}
// remove the lost users if any
if (lostUsers.Count > 0)
{
foreach (uint userId in lostUsers)
{
RemoveUser(userId);
}
lostUsers.Clear();
}
}
// Process the skeleton data
void ProcessSkeleton()
{
List <uint> lostUsers = new List <uint>();
lostUsers.AddRange(allUsers);
// calculate the time since last update
float currentNuiTime = Time.realtimeSinceStartup;
float deltaNuiTime = currentNuiTime - lastNuiTime;
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
uint userId = skeletonData.dwTrackingID;
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
// get the skeleton position
Vector3 skeletonPos = kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
if (!AllPlayersCalibrated)
{
// check if this is the closest user
bool bClosestUser = true;
if (DetectClosestUser)
{
for (int j = 0; j < KinectWrapper.Constants.NuiSkeletonCount; j++)
{
if (j != i)
{
KinectWrapper.NuiSkeletonData skeletonDataOther = skeletonFrame.SkeletonData[j];
if ((skeletonDataOther.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked) &&
(Mathf.Abs(kinectToWorld.MultiplyPoint3x4(skeletonDataOther.Position).z) < Mathf.Abs(skeletonPos.z)))
{
bClosestUser = false;
break;
}
}
}
}
if (bClosestUser)
{
CalibrateUser(userId, i + 1, ref skeletonData);
}
}
if (userId == Player1ID && Mathf.Abs(skeletonPos.z) >= MinUserDistance &&
(MaxUserDistance <= 0f || Mathf.Abs(skeletonPos.z) <= MaxUserDistance))
{
player1Index = i;
// get player position
player1Pos = skeletonPos;
// apply tracking state filter first
trackingStateFilter[0].UpdateFilter(ref skeletonData);
if (UseSelfIntersectionConstraint && selfIntersectionConstraint != null)
{
selfIntersectionConstraint.Constrain(ref skeletonData);
}
// get joints' position and rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
bool playerTracked = IgnoreInferredJoints ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(Array.BinarySearch(mustBeTrackedJoints, j) >= 0 ? (int)skeletonData.eSkeletonPositionTrackingState[j] == stateTracked :
(int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked);
player1JointsTracked[j] = player1PrevTracked[j] && playerTracked;
player1PrevTracked[j] = playerTracked;
if (player1JointsTracked[j])
{
player1JointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
//player1JointsOri[j] = jointOrients[j].absoluteRotation.rotationMatrix * flipMatrix;
}
}
// calculate joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref player1JointsPos, ref player1JointsTracked, ref player1JointsOri);
// filter orientation constraints
if (UseBoneOrientationsConstraint && boneConstraintsFilter != null)
{
boneConstraintsFilter.Constrain(ref player1JointsOri, ref player1JointsTracked);
}
// filter joint orientations.
// it should be performed after all joint position modifications.
if (UseBoneOrientationsFilter && boneOrientationFilter[0] != null)
{
boneOrientationFilter[0].UpdateFilter(ref skeletonData, ref player1JointsOri);
}
// get player rotation
player1Ori = player1JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
lostUsers.Remove(userId);
}
}
// update the nui-timer
lastNuiTime = currentNuiTime;
// remove the lost users if any
if (lostUsers.Count > 0)
{
foreach (uint userId in lostUsers)
{
RemoveUser(userId);
}
lostUsers.Clear();
}
}
// Process the skeleton data
void ProcessSkeleton()
{
List <uint> lostUsers = new List <uint>();
lostUsers.AddRange(allUsers);
for (int i = 0; i < KinectWrapper.Constants.NuiSkeletonCount; i++)
{
KinectWrapper.NuiSkeletonData skeletonData = skeletonFrame.SkeletonData[i];
uint userId = skeletonData.dwTrackingID;
if (skeletonData.eTrackingState == KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
if (!AllPlayersCalibrated)
{
CalibrateUser(userId);
}
int stateNotTracked = (int)KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked;
if (userId == Player1ID)
{
//Debug.Log("user yes:" + userId.ToString()); //Waterstrong
// get player position
player1Pos = kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
player1Pos.z = -player1Pos.z;
//Debug.Log(String.Format("({0}, {1}, {2})", player1Pos.x, player1Pos.y, player1Pos.z));
//CalibrationText.guiText.text = String.Format("({0:F1}, {1:F1}, {2:F1})", player1Pos.x, player1Pos.y, player1Pos.z);
// get joints rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
player1JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked;
if (player1JointsTracked[j])
{
player1JointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
player1JointsPos[j].z = -player1JointsPos[j].z;
}
}
// get joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref player1JointsPos, ref player1JointsTracked, ref player1JointsOri);
// get player rotation
player1Ori = player1JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
// Waterstrong Add UI
//_adaptiveUi.SetUiCenter(player1JointsPos[(int)KinectWrapper.NuiSkeletonPositionIndex.Head], player1JointsPos[(int)KinectWrapper.NuiSkeletonPositionIndex.HandLeft], player1JointsPos[(int)KinectWrapper.NuiSkeletonPositionIndex.HandRight]);
// Waterstrong Add
FeatureData featureData = new FeatureData();
featureData.SetRelativeJoints(player1JointsPos);
MotionType mt = _motion.HandleDataEx(featureData);
switch (mt)
{
case MotionType.Jump:
Debug.Log("jump");
//SendMessage("DidJumpReachApex", SendMessageOptions.DontRequireReceiver);
break;
//case MotionType.Left:
// Debug.Log("left");
// break;
//case MotionType.Right:
// Debug.Log("right");
// break;
default: break;
}
}
else if (userId == Player2ID)
{
// get player position
player2Pos = kinectToWorld.MultiplyPoint3x4(skeletonData.Position);
player2Pos.z = -player2Pos.z;
// get joints rotation
for (int j = 0; j < (int)KinectWrapper.NuiSkeletonPositionIndex.Count; j++)
{
player2JointsTracked[j] = (int)skeletonData.eSkeletonPositionTrackingState[j] != stateNotTracked;
if (player2JointsTracked[j])
{
player2JointsPos[j] = kinectToWorld.MultiplyPoint3x4(skeletonData.SkeletonPositions[j]);
player2JointsPos[j].z = -player2JointsPos[j].z;
}
}
// get joint orientations
KinectWrapper.GetSkeletonJointOrientation(ref player2JointsPos, ref player2JointsTracked, ref player2JointsOri);
// get player rotation
player2Ori = player2JointsOri[(int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter];
}
lostUsers.Remove(userId);
}
}
// remove the lost users if any
if (lostUsers.Count > 0)
{
foreach (uint userId in lostUsers)
{
RemoveUser(userId);
}
lostUsers.Clear();
}
}
