public Status Update(Rotation hipsOrientation)
{
if (!KinectDevice.JointIsTracked(footJoint))
{
status = Status.Present;
return(status);
}
status = Status.Tracking;
Vector hipsPosition = kinectDevice.GetTargetPosition(hipJoint);
Vector kneePosition = kinectDevice.GetTargetPosition(kneeJoint);
Vector anklePosition = kinectDevice.GetTargetPosition(ankleJoint);
//Vector footPosition = kinectDevice.GetTargetPosition(footJoint);
// Position
upperLeg.confidence.position = KinectDevice.TrackingConfidence(upperLeg.position, hipsPosition);
upperLeg.position = hipsPosition;
lowerLeg.confidence.position = KinectDevice.TrackingConfidence(lowerLeg.position, kneePosition);
lowerLeg.position = kneePosition;
foot.confidence.position = KinectDevice.TrackingConfidence(foot.position, anklePosition);
foot.position = anklePosition;
// Rotation
upperLeg.rotation = device.ToWorldOrientation(CalculateLegOrientation(hipsPosition, kneePosition, hipsOrientation));
upperLeg.confidence.rotation = Math.Min(upperLeg.confidence.position, lowerLeg.confidence.position);
lowerLeg.rotation = device.ToWorldOrientation(CalculateLegOrientation(kneePosition, anklePosition, hipsOrientation));
lowerLeg.confidence.rotation = Math.Min(lowerLeg.confidence.position, foot.confidence.position);
foot.rotation = device.ToWorldOrientation(Rotation.AngleAxis(180, Vector.up));
foot.confidence.rotation = foot.confidence.position;
return(status);
}
private void UpdateArm()
{
Vector shoulderPosition = kinectDevice.GetTargetPosition(shoulderJoint);
Vector elbowPosition = kinectDevice.GetTargetPosition(elbowJoint);
Vector wristPosition = kinectDevice.GetTargetPosition(wristJoint);
Vector upperArmDirection = elbowPosition - shoulderPosition;
Vector forearmDirection = wristPosition - elbowPosition;
float upperArmLength = Vector.Magnitude(upperArmDirection);
float forearmLength = Vector.Magnitude(forearmDirection);
Vector elbowUp = Vector.Cross(upperArmDirection, forearmDirection);
if (!isLeft)
{
elbowUp = -elbowUp;
}
upperArm.confidence.position = KinectDevice.TrackingConfidence(upperArm.position, shoulderPosition);
upperArm.position = shoulderPosition;
upperArm.rotation = CalculateUpperArmOrientation(shoulderPosition, upperArmLength, elbowUp, forearmLength, wristPosition, isLeft);
forearm.confidence.position = KinectDevice.TrackingConfidence(forearm.position, elbowPosition);
forearm.position = elbowPosition;
upperArm.confidence.rotation = Math.Min(upperArm.confidence.position, forearm.confidence.position);
//float handPositionConfidence = KinectDevice.TrackingConfidence(hand.position, wristPosition);
//if (handPositionConfidence >= hand.confidence.position) { // Is this tracking info better than we already have?
hand.confidence.position = 0.9F;
Vector thumbPosition = kinectDevice.GetTargetPosition(thumbJoint);
Vector handPosition = kinectDevice.GetTargetPosition(handTipJoint);
Vector handDirection = handPosition - wristPosition;
Vector thumbDirection = wristPosition - thumbPosition;
Vector handUp = Vector.Cross(handDirection, thumbDirection);
if (isLeft)
{
handUp = -handUp;
}
forearm.rotation = CalculateArmOrientation(elbowPosition, elbowUp, wristPosition, isLeft);
forearm.confidence.rotation = Math.Min(forearm.confidence.position, hand.confidence.position);
hand.position = wristPosition;
// hand rotational tracking is quite unreliable
hand.rotation = CalculateArmOrientation(wristPosition, handUp, handPosition, isLeft);
hand.confidence.rotation = 0.6F;
//}
//else {
// float weight = 0.01F;
// hand.confidence.position = handPositionConfidence * weight + hand.confidence.position * (1 - weight);
// hand.position = wristPosition * weight + hand.position * (1 - weight);
// // do not use wristRotation here, quality is low, we expect any other tracker is better
// forearm.rotation = CalculateArmOrientation(elbowPosition, elbowUp, hand.position, isLeft);
// forearm.confidence.rotation = Math.Min(forearm.confidence.position, hand.confidence.position);
//}
}