void Update()
{
if (inputManager.enableKinect)
{
for (int i = 0; i < playerManager.m_MaxNumberOfPlayers; i++)
{
skeletons [kinect1SensorID, i].isTracking = playerManager.GetPlayer(i).Tracking;
if (!skeletons [kinect1SensorID, i].isTracking)
{
continue;
}
UpdateKinectRootData(i);
UpdateKinectJointData(OpenNI.SkeletonJoint.Head, i, ref skeletons [kinect1SensorID, i].head);
UpdateKinectJointData(OpenNI.SkeletonJoint.Torso, i, ref skeletons [kinect1SensorID, i].torso);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftShoulder, i, ref skeletons [kinect1SensorID, i].leftShoulder);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftElbow, i, ref skeletons [kinect1SensorID, i].leftElbow);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftHand, i, ref skeletons [kinect1SensorID, i].leftHand);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightShoulder, i, ref skeletons [kinect1SensorID, i].rightShoulder);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightElbow, i, ref skeletons [kinect1SensorID, i].rightElbow);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightHand, i, ref skeletons [kinect1SensorID, i].rightHand);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftHip, i, ref skeletons [kinect1SensorID, i].leftHip);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftKnee, i, ref skeletons [kinect1SensorID, i].leftKnee);
UpdateKinectJointData(OpenNI.SkeletonJoint.LeftFoot, i, ref skeletons [kinect1SensorID, i].leftFoot);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightHip, i, ref skeletons [kinect1SensorID, i].rightHip);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightKnee, i, ref skeletons [kinect1SensorID, i].rightKnee);
UpdateKinectJointData(OpenNI.SkeletonJoint.RightFoot, i, ref skeletons [kinect1SensorID, i].rightFoot);
}
}
if (inputManager.enableKinect2)
{
bool isNewFrame = false;
Kinect.Body[] data = RUISKinect2Data.getData(out isNewFrame);
isNewKinect2Frame = isNewFrame;
if (data != null && isNewFrame)
{
Vector3 relativePos;
int playerID = 0;
bool newBody = true;
int i = 0;
// Refresh skeleton tracking status
for (int y = 0; y < skeletons.GetLength(1); y++)
{
skeletons [kinect2SensorID, y].isTracking = false;
}
foreach (var body in data)
{
if (body.IsTracked)
{
for (int y = 0; y < skeletons.GetLength(1); y++)
{
if (skeletons [kinect2SensorID, y].trackingId == body.TrackingId)
{
skeletons [kinect2SensorID, y].isTracking = true;
}
}
}
}
foreach (var body in data)
{
if (i >= skeletons.GetLength(1))
{
break;
}
if (body == null)
{
continue;
}
newBody = true;
playerID = 0;
// Check if trackingID has been assigned to certaint index before and use that index
if (trackingIDtoIndex.ContainsKey(body.TrackingId) && body.IsTracked)
{
playerID = trackingIDtoIndex[body.TrackingId];
newBody = false;
}
if (body.IsTracked)
{
if (newBody)
{
// Find the first unused slot in skeletons array
for (int y = 0; y < skeletons.GetLength(1); y++)
{
if (!skeletons [kinect2SensorID, y].isTracking)
{
playerID = y;
break;
}
}
}
trackingIDtoIndex[body.TrackingId] = playerID;
skeletons [kinect2SensorID, playerID].trackingId = body.TrackingId;
// HACK TO make things faster, remove later
// if(playerID > 0)
// return;
// HACK: Kinect 2 can't track closer than 0.5 meters!
if (body.Joints[Kinect.JointType.SpineMid].Position.Z < 0.5f)
{
skeletons [kinect2SensorID, playerID].isTracking = false;
}
UpdateKinect2RootData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.Head], body.JointOrientations[Kinect.JointType.Head]), playerID, ref skeletons [kinect2SensorID, playerID].head);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.Neck], body.JointOrientations[Kinect.JointType.Neck]), playerID, ref skeletons [kinect2SensorID, playerID].neck);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID, ref skeletons [kinect2SensorID, playerID].torso);
// Kinect 2 SpineMid position adjusted to correspond to Kinect 1's torso position (so that hip-torso segment doesn't stretch):
// First get SpineBase position
tempVector = coordinateSystem.ConvertLocation(coordinateSystem.ConvertRawKinect2Location(
GetKinect2JointData(body.Joints[Kinect.JointType.SpineBase],
body.JointOrientations[Kinect.JointType.SpineMid]).position), RUISDevice.Kinect_2);
// tempVector = torsoPosition + offset * (spineBasePosition - torsoPosition)
tempVector = skeletons [kinect2SensorID, playerID].torso.position + torsoOffsetKinect2 * (tempVector - skeletons[kinect2SensorID, playerID].torso.position);
skeletons[kinect2SensorID, playerID].root.position = tempVector;
skeletons[kinect2SensorID, playerID].torso.position = tempVector;
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID, ref skeletons [kinect2SensorID, playerID].midSpine);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineShoulder], body.JointOrientations[Kinect.JointType.SpineShoulder]), playerID, ref skeletons [kinect2SensorID, playerID].shoulderSpine);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ShoulderLeft], body.JointOrientations[Kinect.JointType.ShoulderLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftShoulder);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ShoulderRight], body.JointOrientations[Kinect.JointType.ShoulderRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightShoulder);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ElbowRight], body.JointOrientations[Kinect.JointType.ElbowRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightElbow);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ElbowLeft], body.JointOrientations[Kinect.JointType.ElbowLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftElbow);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandRight], body.JointOrientations[Kinect.JointType.HandRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHand);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandLeft], body.JointOrientations[Kinect.JointType.HandLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHand);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HipLeft], body.JointOrientations[Kinect.JointType.HipLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHip);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HipRight], body.JointOrientations[Kinect.JointType.HipRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHip);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandTipRight], body.JointOrientations[Kinect.JointType.HandTipRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHandTip);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandTipLeft], body.JointOrientations[Kinect.JointType.HandTipLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHandTip);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.KneeRight], body.JointOrientations[Kinect.JointType.KneeRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightKnee);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.KneeLeft], body.JointOrientations[Kinect.JointType.KneeLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftKnee);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.WristLeft], body.JointOrientations[Kinect.JointType.WristLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftWrist);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.WristRight], body.JointOrientations[Kinect.JointType.WristRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightWrist);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.AnkleLeft], body.JointOrientations[Kinect.JointType.AnkleLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftAnkle);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.AnkleRight], body.JointOrientations[Kinect.JointType.AnkleRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightAnkle);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandLeft], body.JointOrientations[Kinect.JointType.HandLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHand);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandRight], body.JointOrientations[Kinect.JointType.HandRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHand);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ThumbLeft], body.JointOrientations[Kinect.JointType.ThumbLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftThumb);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ThumbRight], body.JointOrientations[Kinect.JointType.ThumbRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightThumb);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.FootLeft], body.JointOrientations[Kinect.JointType.FootLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftFoot);
UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.FootRight], body.JointOrientations[Kinect.JointType.FootRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightFoot);
/*
* Rotation corrections
* Map rotations to Kinect 1 space (which is currently assumed by every script that uses RUISSkeletonManager)
* HACK: Below joints are accessed from top to bottom in hierarchy, because we modify some values before using them later:
* E.g. shoulder rotations are modified first because they are used to calculate lower arm rotations (same with hands and thumbs).
* Also funky reassignments: skeleton[...].leftHip.rotation = skeleton[...].leftKnee.rotation
* TODO: These should be fixed to be less confusing and error-inducing, preferably already in above UpdateKinect2JointData() assignments
*/
// *** TODO: Check that avatar rotations match that of Kinect 2 demo with boxes as body parts. Not sure about shoulders, hands, and thumbs
// Upper arm 45 degree rotation ??
// Head
relativePos = skeletons [kinect2SensorID, playerID].head.position - skeletons [kinect2SensorID, playerID].neck.position;
skeletons [kinect2SensorID, playerID].head.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, -90, -90);
// Torso
relativePos = skeletons [kinect2SensorID, playerID].midSpine.position - skeletons [kinect2SensorID, playerID].shoulderSpine.position;
skeletons [kinect2SensorID, playerID].torso.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90); // TODO: Bug when turning right
// Upper leg
skeletons [kinect2SensorID, playerID].leftHip.rotation = skeletons [kinect2SensorID, playerID].leftKnee.rotation * Quaternion.Euler(180, -90, 0);
skeletons [kinect2SensorID, playerID].rightHip.rotation = skeletons [kinect2SensorID, playerID].rightKnee.rotation * Quaternion.Euler(180, 90, 0);;
// Lower leg
relativePos = skeletons [kinect2SensorID, playerID].leftAnkle.position - skeletons [kinect2SensorID, playerID].leftKnee.position;
skeletons [kinect2SensorID, playerID].leftKnee.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90);
// skeletons [kinect2SensorID, playerID].leftKnee.rotation = skeletons [kinect2SensorID, playerID].leftAnkle.rotation * Quaternion.Euler(180, -90, 0);
relativePos = skeletons [kinect2SensorID, playerID].rightAnkle.position - skeletons [kinect2SensorID, playerID].rightKnee.position;
skeletons [kinect2SensorID, playerID].rightKnee.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90);
// skeletons [kinect2SensorID, playerID].rightKnee.rotation = skeletons [kinect2SensorID, playerID].rightAnkle.rotation * Quaternion.Euler(180, 90, 0);;
// Feet / Ankles
relativePos = skeletons [kinect2SensorID, playerID].leftAnkle.position - skeletons [kinect2SensorID, playerID].leftFoot.position;
skeletons [kinect2SensorID, playerID].leftFoot.rotation = Quaternion.LookRotation(relativePos) * Quaternion.Euler(0, 180, 0);
relativePos = skeletons [kinect2SensorID, playerID].rightAnkle.position - skeletons [kinect2SensorID, playerID].rightFoot.position;
skeletons [kinect2SensorID, playerID].rightFoot.rotation = Quaternion.LookRotation(relativePos) * Quaternion.Euler(0, 180, 0);
// Upper arm
//relativePos = skeletons [kinect2SensorID, playerID].leftElbow.position - skeletons [kinect2SensorID, playerID].leftShoulder.position;
//skeletons [kinect2SensorID, playerID].leftShoulder.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].leftElbow.rotation * Vector3.right) * Quaternion.Euler(-45, 90, 0);
skeletons [kinect2SensorID, playerID].leftShoulder.rotation = skeletons [kinect2SensorID, playerID].leftElbow.rotation * Quaternion.Euler(0, 45, -90);
//relativePos = skeletons [kinect2SensorID, playerID].rightElbow.position - skeletons [kinect2SensorID, playerID].rightShoulder.position;
//skeletons [kinect2SensorID, playerID].rightShoulder.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].rightElbow.rotation * Vector3.right) * Quaternion.Euler(135, 270, 0);
skeletons [kinect2SensorID, playerID].rightShoulder.rotation = skeletons [kinect2SensorID, playerID].rightElbow.rotation * Quaternion.Euler(0, -45, 90);
// Lower arm
relativePos = skeletons [kinect2SensorID, playerID].leftElbow.position - skeletons [kinect2SensorID, playerID].leftWrist.position;
skeletons [kinect2SensorID, playerID].leftElbow.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].leftShoulder.rotation * Vector3.forward) * Quaternion.Euler(180, -90, 0);
//skeletons [kinect2SensorID, playerID].leftElbow.rotation = skeletons [kinect2SensorID, playerID].leftWrist.rotation * Quaternion.Euler(0, 180, -90);
relativePos = skeletons [kinect2SensorID, playerID].rightElbow.position - skeletons [kinect2SensorID, playerID].rightWrist.position;
skeletons [kinect2SensorID, playerID].rightElbow.rotation = Quaternion.LookRotation(relativePos,
skeletons [kinect2SensorID, playerID].rightShoulder.rotation * Vector3.forward) * Quaternion.Euler(180, 90, 0);
//skeletons [kinect2SensorID, playerID].rightElbow.rotation = skeletons [kinect2SensorID, playerID].rightWrist.rotation * Quaternion.Euler(0, -180, 90);
// Hands
skeletons [kinect2SensorID, playerID].leftHand.rotation *= Quaternion.Euler(0, 180, -90);
skeletons [kinect2SensorID, playerID].rightHand.rotation *= Quaternion.Euler(0, 180, 90);
// Thumbs
relativePos = skeletons [kinect2SensorID, playerID].leftThumb.position - skeletons[kinect2SensorID, playerID].leftWrist.position;
skeletons [kinect2SensorID, playerID].leftThumb.rotation = Quaternion.LookRotation(relativePos,
skeletons[kinect2SensorID, playerID].leftHand.rotation * Vector3.up)
* Quaternion.Euler(-90, 0, skeletons[kinect2SensorID, playerID].thumbZRotationOffset);
relativePos = skeletons [kinect2SensorID, playerID].rightThumb.position - skeletons[kinect2SensorID, playerID].rightWrist.position;
skeletons [kinect2SensorID, playerID].rightThumb.rotation = Quaternion.LookRotation(relativePos,
skeletons[kinect2SensorID, playerID].rightHand.rotation * Vector3.up)
* Quaternion.Euler(-90, 0, -skeletons[kinect2SensorID, playerID].thumbZRotationOffset);
// Fist curling
switch (body.HandLeftState)
{
case Kinect.HandState.Closed:
skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.closed;
break;
case Kinect.HandState.Open:
skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.open;
break;
case Kinect.HandState.Lasso:
skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.pointing;
break;
case Kinect.HandState.Unknown:
case Kinect.HandState.NotTracked:
skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.unknown;
break;
}
switch (body.HandRightState)
{
case Kinect.HandState.Closed:
skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.closed;
break;
case Kinect.HandState.Open:
skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.open;
break;
case Kinect.HandState.Lasso:
skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.pointing;
break;
case Kinect.HandState.Unknown:
case Kinect.HandState.NotTracked:
skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.unknown;
break;
}
// HACK for filtering Kinect 2 arm rotations
// TODO: More efficient rotation filtering, extend filtering to all joints (now just arms)
if (isNewKinect2Frame && skeletons[kinect2SensorID, playerID].filterRotations)
{
float kalmanDeltaTime = 0.033f;
//if(skeletons[kinect2SensorID, playerID].leftShoulder.rotationConfidence >= 0.5f)
{
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftShoulder, 0, kalmanDeltaTime);
}
//if(skeletons[kinect2SensorID, playerID].rightShoulder.rotationConfidence >= 0.5f)
{
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightShoulder, 1, kalmanDeltaTime);
}
//if(skeletons[kinect2SensorID, playerID].leftElbow.rotationConfidence >= 0.5f)
{
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftElbow, 2, kalmanDeltaTime);
}
//if(skeletons[kinect2SensorID, playerID].rightElbow.rotationConfidence >= 0.5f)
{
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightElbow, 3, kalmanDeltaTime);
}
// filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].root, 4, kalmanDeltaTime);
//
//// measuredRotation = skeletons[kinect2SensorID, playerID].root.rotation;
//// skeletons[kinect2SensorID, playerID].root.rotation = skeletons[kinect2SensorID, playerID].filterRot[4].Update(measuredRotation, kalmanDeltaTime);
//
// TODO uncomment
// filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].torso, 5, kalmanDeltaTime);
//
//// measuredRotation = skeletons[kinect2SensorID, playerID].torso.rotation;
//// skeletons[kinect2SensorID, playerID].torso.rotation = skeletons[kinect2SensorID, playerID].filterRot[5].Update(measuredRotation, kalmanDeltaTime);
//
// TODO uncomment
// filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].midSpine, 6, kalmanDeltaTime);
//
//// measuredRotation = skeletons[kinect2SensorID, playerID].midSpine.rotation;
//// skeletons[kinect2SensorID, playerID].midSpine.rotation = skeletons[kinect2SensorID, playerID].filterRot[6].Update(measuredRotation, kalmanDeltaTime);
//
// TODO uncomment
// filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].shoulderSpine, 7, kalmanDeltaTime);
//
//// measuredRotation = skeletons[kinect2SensorID, playerID].shoulderSpine.rotation;
//// skeletons[kinect2SensorID, playerID].shoulderSpine.rotation = skeletons[kinect2SensorID, playerID].filterRot[7].Update(measuredRotation, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftHand, 8, kalmanDeltaTime);
// measuredRotation = skeletons[kinect2SensorID, playerID].leftHand.rotation;
// skeletons[kinect2SensorID, playerID].leftHand.rotation = skeletons[kinect2SensorID, playerID].filterRot[8].Update(measuredRotation, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightHand, 9, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightHip, 10, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftHip, 11, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightKnee, 12, kalmanDeltaTime);
filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftKnee, 13, kalmanDeltaTime);
// measuredRotation = skeletons[kinect2SensorID, playerID].rightHand.rotation;
// skeletons[kinect2SensorID, playerID].rightHand.rotation = skeletons[kinect2SensorID, playerID].filterRot[9].Update(measuredRotation, kalmanDeltaTime);
// Kalman filtering is a slightly heavy operation, just average two latest thumb rotations // TODO uncomment
// Quaternion thumbRotation = skeletons[kinect2SensorID, playerID].leftThumb.rotation;
// skeletons[kinect2SensorID, playerID].leftThumb.rotation = Quaternion.Slerp(thumbRotation,
// skeletons[kinect2SensorID, playerID].previousRotation[10], 0.5f);
// skeletons[kinect2SensorID, playerID].previousRotation[10] = thumbRotation;
//
// thumbRotation = skeletons[kinect2SensorID, playerID].rightThumb.rotation;
// skeletons[kinect2SensorID, playerID].rightThumb.rotation = Quaternion.Slerp(thumbRotation,
// skeletons[kinect2SensorID, playerID].previousRotation[11], 0.5f);
// skeletons[kinect2SensorID, playerID].previousRotation[11] = thumbRotation;
}
i++;
}
}
}
if (isNewKinect2Frame)
{
kinect2FrameDeltaT = timeSinceLastKinect2Frame;
timeSinceLastKinect2Frame = 0;
}
else
{
timeSinceLastKinect2Frame += Time.deltaTime;
}
}
}
private Vector3 getSample(RUISDevice device) {
Vector3 sample = new Vector3(0,0,0);
Vector3 tempSample;
updateBodyData();
if(device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach(var body in data)
{
foundBodies++;
if(body.IsTracked)
{
if(trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if(body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if(Vector3.Distance(tempSample, lastKinect2Sample) > 0.1)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if(!device2Error) this.guiTextUpperLocal = "";
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if(!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if(device == RUISDevice.PS_Move)
{
if(psMoveWrapper.sphereVisible[calibratingPSMoveControllerId] &&
psMoveWrapper.handleVelocity[calibratingPSMoveControllerId].magnitude <= 10.0f)
{
tempSample = coordinateSystem.ConvertRawPSMoveLocation(psMoveWrapper.handlePosition[calibratingPSMoveControllerId]);
if(Vector3.Distance(tempSample, lastPSMoveSample) > 0.1)
{
sample = tempSample;
lastPSMoveSample = sample;
device2Error = false;
if(!device1Error) this.guiTextUpperLocal = "";
}
else {
device2Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
return sample;
}
private Vector3 getSample(RUISDevice device)
{
Vector3 sample = new Vector3(0, 0, 0);
Vector3 tempSample;
updateBodyData();
if (device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach (var body in data)
{
foundBodies++;
if (body.IsTracked)
{
if (trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if (body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if (Vector3.Distance(tempSample, lastKinect2Sample) > 0.1)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if (!device2Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if (!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if (device == RUISDevice.Oculus_DK2)
{
Ovr.Posef headpose = RUISOVRManager.ovrHmd.GetTrackingState().HeadPose.ThePose;
float px = headpose.Position.x;
float py = headpose.Position.y;
float pz = -headpose.Position.z; // This needs to be negated TODO: might change with future OVR version
tempSample = new Vector3(px, py, pz);
tempSample = coordinateSystem.ConvertRawOculusDK2Location(tempSample);
if ((Vector3.Distance(tempSample, lastOculusDK2Sample) > 0.1) &&
(RUISOVRManager.ovrHmd.GetTrackingState().StatusFlags & (uint)StatusBits.PositionTracked) != 0) // Code from OVRManager.cs
{
sample = tempSample;
lastOculusDK2Sample = sample;
device2Error = false;
if (!device1Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device2Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
return(sample);
}
private Vector3 getSample(RUISDevice device)
{
Vector3 sample = new Vector3(0, 0, 0);
Vector3 tempSample;
updateBodyData();
if (device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach (var body in data)
{
foundBodies++;
if (body.IsTracked)
{
if (trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if (body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if (Vector3.Distance(tempSample, lastKinect2Sample) > 0.2)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if (!device2Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if (!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if (device == RUISDevice.Kinect_1)
{
OpenNI.SkeletonJointPosition jointPosition;
bool success = kinectSelection.GetPlayer(0).GetSkeletonJointPosition(OpenNI.SkeletonJoint.RightHand, out jointPosition);
if (success && jointPosition.Confidence >= 0.5)
{
tempSample = coordinateSystem.ConvertRawKinectLocation(jointPosition.Position);
if (Vector3.Distance(tempSample, lastKinectSample) > 0.2)
{
sample = tempSample;
lastKinectSample = sample;
device2Error = false;
if (!device1Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device2Error = true;
// this.guiTextUpperLocal = "Not enough hand movement.";
}
}
else
{
device2Error = true;
// this.guiTextUpperLocal = "Not enough hand movement.";
}
}
return(sample);
}
