void BodyFrameReaderOnFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (var frame = e.FrameReference.AcquireFrame())
{
if (bodies == null)
{
bodies = new Body[frame.BodyCount];
}
frame.GetAndRefreshBodyData(bodies);
var trackedBody = bodies.FirstOrDefault(body => body.IsTracked);
if (trackedBody == null)
{
return;
}
jointFilter.UpdateFilter(trackedBody);
var filteredJoints = jointFilter.GetFilteredJoints();
var joints = new Dictionary <BodyJointType, BodyJoint>();
for (var index = 0; index < filteredJoints.Length; index++)
{
var joint = filteredJoints[index];
joints[(BodyJointType)index] = new BodyJoint
{
Position = new Vec3 {
X = joint.X, Y = joint.Y, Z = joint.Z
}
};
}
foreach (var jointOrientation in trackedBody.JointOrientations.Values)
{
joints[(BodyJointType)jointOrientation.JointType].Orientation = new Vec4
{
X = jointOrientation.Orientation.X,
Y = jointOrientation.Orientation.Y,
Z = jointOrientation.Orientation.Z,
W = jointOrientation.Orientation.W
}
}
;
listener.OnBodyFrameReceived(joints);
}
}
}
/*
* converts spatial information to joint relative rotations
* -1 for right, 1 for left
*/
Quaternion _convertLocToRot(BodyJoint.JointType jType, BodyJoint.JointType jChild, Quaternion parentAtt, float dir)
{
Vector3 tForward = Quaternion.AngleAxis(180, Vector3.up) * (getJointLoc(jChild) - getJointLoc(jType));
Vector3 tRight = -Vector3.right;
Vector3 tUp = Vector3.Cross(tRight.normalized, tForward.normalized);
Quaternion tAtt = Quaternion.LookRotation(tForward.normalized, tUp.normalized) * Quaternion.AngleAxis(dir*90, Vector3.up);
setJointRot(jType, Quaternion.Inverse(parentAtt) * tAtt);
return tAtt;
}
public Quaternion setJointRot(BodyJoint.JointType type, Quaternion rot)
{
joint_rotations[(int)type] = rot;
return rot;
}
public Quaternion getJointRot(BodyJoint.JointType type)
{
return (Quaternion)joint_rotations[(int)type];
}
public Vector3 getJointLoc(BodyJoint.JointType type)
{
return joint_locations[(int)type];
}
public void InitBody(NiteController niteController) {
Debug.Log("Initializing the user body");
niteController.Update(); // NiteWrapper does not have the new transforms yet
// TODO: get radius
float upperLegRadius = 0.075F, lowerLegRadius = 0.06F, neckRadius = 0.05F;
//get positions of left arm parts
Vector3 leftShoulderPos, leftElbowPos, leftHandPos;
Quaternion leftShoulderRot, leftElbowRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, out leftShoulderPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_ELBOW, out leftElbowPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_HAND, out leftHandPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, out leftShoulderRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_ELBOW, out leftElbowRot);
//intialize left arm components
float leftArmUpperRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_UPPER_2];
float leftArmLowerRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_LOWER_2];
if (leftArmUpperRadius > 0.07F){
leftArmUpperRadius = 0.07F;
}
if (leftArmLowerRadius > 0.06F){
leftArmLowerRadius = 0.06F;
}
leftShoulder = new BodyJoint("Left_Shoulder", leftShoulderPos, leftShoulderRot, Vector3.left, (leftElbowPos - leftShoulderPos).magnitude, leftArmUpperRadius);
leftElbow = new BodyJoint("Left_Elbow", leftElbowPos, leftElbowRot, Vector3.left, (leftHandPos - leftElbowPos).magnitude, leftArmLowerRadius);
leftElbow.transform.parent = leftShoulder.transform;
//get positions of right arm parts
Vector3 rightShoulderPos, rightElbowPos, rightHandPos;
Quaternion rightShoulderRot, rightElbowRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, out rightShoulderPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, out rightElbowPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_HAND, out rightHandPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, out rightShoulderRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, out rightElbowRot);
//intialize right arm components
float rightArmUpperRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_UPPER_2];
float rightArmLowerRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_LOWER_2];
if (rightArmUpperRadius > 0.07F){
rightArmUpperRadius = 0.07F;
}
if (rightArmLowerRadius > 0.06F){
rightArmLowerRadius = 0.06F;
}
rightShoulder = new BodyJoint("Right_Shoulder", rightShoulderPos, rightShoulderRot, Vector3.right, (rightElbowPos - rightShoulderPos).magnitude, rightArmUpperRadius);
rightElbow = new BodyJoint("Right_Elbow", rightElbowPos, rightElbowRot, Vector3.right, (rightHandPos - rightElbowPos).magnitude, rightArmLowerRadius);
rightElbow.transform.parent = rightShoulder.transform;
//get positions of left leg parts
Vector3 leftHipPos, leftKneePos, leftFootPos;
Quaternion leftHipRot, leftKneeRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_HIP, out leftHipPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_KNEE, out leftKneePos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_FOOT, out leftFootPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_HIP, out leftHipRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_KNEE, out leftKneeRot);
//intialize left leg components
leftHip = new BodyJoint("Left_Hip", leftHipPos, leftHipRot, Vector3.down, (leftKneePos - leftHipPos).magnitude, upperLegRadius);
leftKnee = new BodyJoint("Left_Knee", leftKneePos, leftKneeRot, Vector3.down, (leftFootPos - leftKneePos).magnitude, lowerLegRadius);
leftKnee.transform.parent = leftHip.transform;
//get positions of right leg parts
Vector3 rightHipPos, rightKneePos, rightFootPos;
Quaternion rightHipRot, rightKneeRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_HIP, out rightHipPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_KNEE, out rightKneePos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_FOOT, out rightFootPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_HIP, out rightHipRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_KNEE, out rightKneeRot);
//intialize right leg components
rightHip = new BodyJoint("Right_Hip", rightHipPos, rightHipRot, Vector3.down, (rightKneePos - rightHipPos).magnitude, upperLegRadius);
rightKnee = new BodyJoint("Right_Knee", rightKneePos, rightKneeRot, Vector3.down, (rightFootPos - rightKneePos).magnitude, lowerLegRadius);
rightKnee.transform.parent = rightHip.transform;
//get positions of neck and head
Vector3 neckPos, headPos;
Quaternion neckRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.NECK, out neckPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.HEAD, out headPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.NECK, out neckRot);
//initialize neck joint
neck = new BodyJoint("Neck", neckPos, neckRot, Vector3.up, (headPos - neckPos).magnitude, neckRadius);
//get positions of torso
Vector3 torsoPos;
Quaternion torsoRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.TORSO_CENTER, out torsoPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.TORSO_CENTER, out torsoRot);
//initialize torso
float shoulderRadius = (0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_UPPER_1] +
0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_UPPER_1]) / 2.0F;
float upperLength, lowerLength;
upperLength = (neckPos - torsoPos).magnitude;
lowerLength = (leftHipPos + 0.5F * (rightHipPos - leftHipPos) - torsoPos).magnitude; //from centerpoint between the two hips to torso_center
AdaptableTorso torso = new AdaptableTorso(torsoPos, torsoRot, upperLength, lowerLength, shoulderRadius);
torso.AdaptToUser(niteController);
torsoCenter = (BodyJoint)torso;
neck.transform.parent = torsoCenter.transform;
leftShoulder.transform.parent = torsoCenter.transform;
rightShoulder.transform.parent = torsoCenter.transform;
leftHip.transform.parent = torsoCenter.transform;
rightHip.transform.parent = torsoCenter.transform;
skeletonJointMapping = new Dictionary<NiteWrapper.SkeletonJoint, BodyJoint>();
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.HEAD, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.NECK, neck);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, leftShoulder);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_ELBOW, leftElbow);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_HAND, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, rightShoulder);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, rightElbow);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_HAND, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.TORSO_CENTER, torsoCenter);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_HIP, leftHip);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_KNEE, leftKnee);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_FOOT, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_HIP, rightHip);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_KNEE, rightKnee);
size = skeletonJointMapping.Count;
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_FOOT, new BodyJoint());
}
public void InitBody(NiteController niteController)
{
Debug.Log("Initializing the user body");
niteController.Update(); // NiteWrapper does not have the new transforms yet
// TODO: get radius
float upperLegRadius = 0.075F, lowerLegRadius = 0.06F, neckRadius = 0.05F;
//get positions of left arm parts
Vector3 leftShoulderPos, leftElbowPos, leftHandPos;
Quaternion leftShoulderRot, leftElbowRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, out leftShoulderPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_ELBOW, out leftElbowPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_HAND, out leftHandPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, out leftShoulderRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_ELBOW, out leftElbowRot);
//intialize left arm components
float leftArmUpperRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_UPPER_2];
float leftArmLowerRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_LOWER_2];
if (leftArmUpperRadius > 0.07F)
{
leftArmUpperRadius = 0.07F;
}
if (leftArmLowerRadius > 0.06F)
{
leftArmLowerRadius = 0.06F;
}
leftShoulder = new BodyJoint("Left_Shoulder", leftShoulderPos, leftShoulderRot, Vector3.left, (leftElbowPos - leftShoulderPos).magnitude, leftArmUpperRadius);
leftElbow = new BodyJoint("Left_Elbow", leftElbowPos, leftElbowRot, Vector3.left, (leftHandPos - leftElbowPos).magnitude, leftArmLowerRadius);
leftElbow.transform.parent = leftShoulder.transform;
//get positions of right arm parts
Vector3 rightShoulderPos, rightElbowPos, rightHandPos;
Quaternion rightShoulderRot, rightElbowRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, out rightShoulderPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, out rightElbowPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_HAND, out rightHandPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, out rightShoulderRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, out rightElbowRot);
//intialize right arm components
float rightArmUpperRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_UPPER_2];
float rightArmLowerRadius = 0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_LOWER_2];
if (rightArmUpperRadius > 0.07F)
{
rightArmUpperRadius = 0.07F;
}
if (rightArmLowerRadius > 0.06F)
{
rightArmLowerRadius = 0.06F;
}
rightShoulder = new BodyJoint("Right_Shoulder", rightShoulderPos, rightShoulderRot, Vector3.right, (rightElbowPos - rightShoulderPos).magnitude, rightArmUpperRadius);
rightElbow = new BodyJoint("Right_Elbow", rightElbowPos, rightElbowRot, Vector3.right, (rightHandPos - rightElbowPos).magnitude, rightArmLowerRadius);
rightElbow.transform.parent = rightShoulder.transform;
//get positions of left leg parts
Vector3 leftHipPos, leftKneePos, leftFootPos;
Quaternion leftHipRot, leftKneeRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_HIP, out leftHipPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_KNEE, out leftKneePos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.LEFT_FOOT, out leftFootPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_HIP, out leftHipRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.LEFT_KNEE, out leftKneeRot);
//intialize left leg components
leftHip = new BodyJoint("Left_Hip", leftHipPos, leftHipRot, Vector3.down, (leftKneePos - leftHipPos).magnitude, upperLegRadius);
leftKnee = new BodyJoint("Left_Knee", leftKneePos, leftKneeRot, Vector3.down, (leftFootPos - leftKneePos).magnitude, lowerLegRadius);
leftKnee.transform.parent = leftHip.transform;
//get positions of right leg parts
Vector3 rightHipPos, rightKneePos, rightFootPos;
Quaternion rightHipRot, rightKneeRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_HIP, out rightHipPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_KNEE, out rightKneePos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.RIGHT_FOOT, out rightFootPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_HIP, out rightHipRot);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.RIGHT_KNEE, out rightKneeRot);
//intialize right leg components
rightHip = new BodyJoint("Right_Hip", rightHipPos, rightHipRot, Vector3.down, (rightKneePos - rightHipPos).magnitude, upperLegRadius);
rightKnee = new BodyJoint("Right_Knee", rightKneePos, rightKneeRot, Vector3.down, (rightFootPos - rightKneePos).magnitude, lowerLegRadius);
rightKnee.transform.parent = rightHip.transform;
//get positions of neck and head
Vector3 neckPos, headPos;
Quaternion neckRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.NECK, out neckPos);
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.HEAD, out headPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.NECK, out neckRot);
//initialize neck joint
neck = new BodyJoint("Neck", neckPos, neckRot, Vector3.up, (headPos - neckPos).magnitude, neckRadius);
//get positions of torso
Vector3 torsoPos;
Quaternion torsoRot;
niteController.GetJointPosition(NiteWrapper.SkeletonJoint.TORSO_CENTER, out torsoPos);
niteController.GetJointOrientation(NiteWrapper.SkeletonJoint.TORSO_CENTER, out torsoRot);
//initialize torso
float shoulderRadius = (0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.LEFT_ARM_UPPER_1] +
0.5F * niteController.diameter[(int)NiteWrapper.BodySlice.RIGHT_ARM_UPPER_1]) / 2.0F;
float upperLength, lowerLength;
upperLength = (neckPos - torsoPos).magnitude;
lowerLength = (leftHipPos + 0.5F * (rightHipPos - leftHipPos) - torsoPos).magnitude; //from centerpoint between the two hips to torso_center
AdaptableTorso torso = new AdaptableTorso(torsoPos, torsoRot, upperLength, lowerLength, shoulderRadius);
torso.AdaptToUser(niteController);
torsoCenter = (BodyJoint)torso;
neck.transform.parent = torsoCenter.transform;
leftShoulder.transform.parent = torsoCenter.transform;
rightShoulder.transform.parent = torsoCenter.transform;
leftHip.transform.parent = torsoCenter.transform;
rightHip.transform.parent = torsoCenter.transform;
skeletonJointMapping = new Dictionary <NiteWrapper.SkeletonJoint, BodyJoint>();
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.HEAD, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.NECK, neck);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_SHOULDER, leftShoulder);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_ELBOW, leftElbow);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_HAND, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_SHOULDER, rightShoulder);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_ELBOW, rightElbow);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_HAND, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.TORSO_CENTER, torsoCenter);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_HIP, leftHip);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_KNEE, leftKnee);
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.LEFT_FOOT, new BodyJoint());
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_HIP, rightHip);
skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_KNEE, rightKnee);
size = skeletonJointMapping.Count;
// skeletonJointMapping.Add(NiteWrapper.SkeletonJoint.RIGHT_FOOT, new BodyJoint());
}
