public static Rotation CalculateUpperArmOrientation(Vector upperArmPosition, float upperArmLength, Vector forearmUp, float forearmLength, Vector handPosition, bool isLeft)
{
Rotation upperArmRotation = Rotation.LookRotation(handPosition - upperArmPosition, forearmUp);
float upperArm2HandDistance = Vector.Distance(upperArmPosition, handPosition);
float upperArm2HandDistance2 = upperArm2HandDistance * upperArm2HandDistance;
float upperArmLength2 = upperArmLength * upperArmLength;
float forearmLength2 = forearmLength * forearmLength;
float elbowAngle = (float)Math.Acos((upperArm2HandDistance2 + upperArmLength2 - forearmLength2) / (2 * upperArm2HandDistance * upperArmLength)) * Rad2Deg;
if (float.IsNaN(elbowAngle))
{
elbowAngle = 10;
}
if (isLeft)
{
elbowAngle = -elbowAngle;
}
upperArmRotation = Rotation.AngleAxis(elbowAngle, upperArmRotation * Vector.up) * upperArmRotation;
if (isLeft)
{
upperArmRotation *= Rotation.AngleAxis(90, Vector.up); // Euler(0, 90, 0)
}
else
{
upperArmRotation *= Rotation.AngleAxis(-90, Vector.up); // Euler(0, -90, 0)
}
return(upperArmRotation);
}
public static Vector GetNeckLocalFacePoint(int facePoint)
{
Rotation faceOrientation = Rotation.AngleAxis(180, Vector.up) * GetFaceOrientation();
Vector faceTargetPosition = ToWorldPosition(GetFacePosition());
Vector worldFacePoint = ToWorldPosition(GetFacePoint(facePoint));
Vector neckLocalFacePoint = worldFacePoint + faceTargetPosition;
neckLocalFacePoint = Rotation.Inverse(faceOrientation) * neckLocalFacePoint;
return(neckLocalFacePoint);
}
private void UpdateJaw()
{
float jawOpen = KinectDevice.GetFaceExpression(KinectDevice.FaceExpression.JawOpen);
float jawAngleX = minAngles.x + jawOpen * (maxAngles.x - minAngles.x);
Rotation newRotation = Rotation.AngleAxis(jawAngleX, Vector.right);
jaw.rotation = Rotation.Slerp(newRotation, jaw.rotation, beta);
jaw.confidence.rotation = 0.7F;
}
public static Rotation GetFaceOrientation()
{
Rotation faceOrientation = new Rotation(
faceAlignment.FaceOrientation.X,
faceAlignment.FaceOrientation.Y,
faceAlignment.FaceOrientation.Z,
faceAlignment.FaceOrientation.W);
Vector faceAngles = Rotation.ToAngles(Rotation.AngleAxis(180, Vector.up) * faceOrientation);
Rotation newOrientation = Rotation_.Euler(-faceAngles.x, -faceAngles.y, faceAngles.z);
return(newOrientation);
}
public static Rotation CalculateArmOrientation(Vector joint1Position, Vector joint1Up, Vector joint2Position, bool isLeft)
{
Vector boneForward = joint2Position - joint1Position;
Rotation boneRotation = Rotation.LookRotation(boneForward, joint1Up);
if (isLeft)
{
boneRotation *= Rotation.AngleAxis(90, Vector.up); // Euler(0, 90, 0)
}
else
{
boneRotation *= Rotation.AngleAxis(-90, Vector.up); // Euler(0, -90, 0)
}
return(boneRotation);
}
public static Rotation GetHeadOrientation()
{
if (faceIsTracked)
{
return(GetFaceOrientation());
}
else
{
Vector neckPos = GetTargetPosition(JointID.Neck);
Vector headPos = GetTargetPosition(JointID.Head);
Vector direction = headPos - neckPos;
direction = Rotation.AngleAxis(180, Vector.up) * direction;
Rotation neckRotation = Rotation.LookRotation(direction, Vector.forward);
neckRotation *= Rotation.AngleAxis(90, Vector.right);
return(neckRotation);
}
}
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);
}
public static Vector GetTargetPosition(JointID boneType)
{
return(Rotation.AngleAxis(180, Vector.up) * GetTargetLocalPosition(boneType));
}
public static Vector Target2WorldPosition(Vector localPosition)
{
//return position + orientation
return(Rotation.AngleAxis(180, Vector.up) * localPosition);
}
