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()); }