public Body() { this.IsTracked = true; this.Joints = new Dictionary <JointType, Joint>(); this.JointOrientations = new Dictionary <JointType, JointOrientation>(); foreach (JointType jointType in Enum.GetValues(typeof(JointType))) { Joints[jointType] = new Joint(); JointOrientations[jointType] = new JointOrientation(); } }
void UpdateJoints(Transform parent, Vector3 totalTrans) { foreach (Transform child in parent) { //Debug.Log(child.name); if (kinectJointIDX.ContainsKey(child.name)) { //Transform JointTransform = BodySourceView.JointTransforms[kinectJointIDX[child.name]]; Windows.Kinect.JointOrientation JointOrientation = BodySourceView.JointOrientations[kinectJointIDX[child.name]]; //Vector3 kinectPos = (JointTransform != null) ? JointTransform.localPosition : child.position; Quaternion kinectRot = (JointOrientation != null) ? new Quaternion(JointOrientation.Orientation.X, JointOrientation.Orientation.Y, JointOrientation.Orientation.Z, JointOrientation.Orientation.W) : child.rotation; //kinectRot = Quaternion.Inverse(parent.rotation) * kinectRot; Vector3 eulerRot = kinectRot.eulerAngles; eulerRot = new Vector3(eulerRot.z, eulerRot.y, eulerRot.x); kinectRot = Quaternion.Euler(eulerRot); // Vector3 rotateVec = kinectPos-parent.position; // kinectRot = Quaternion.LookRotation(rotateVec, Vector3.up); //kinectPos.z = -kinectPos.z; //Debug.Log(child.name + kinectPos); //child.position = kinectPos;// - totalTrans; child.localRotation = kinectRot; //Debug.Log(rotateVec); } else { child.rotation = parent.rotation; } UpdateJoints(child, child.position + totalTrans); } }
public bool Equals(JointOrientation obj) { return JointType.Equals(obj.JointType) && Orientation.Equals(obj.Orientation); }
// Token: 0x06002D1E RID: 11550 RVA: 0x000DF3F8 File Offset: 0x000DD7F8 public bool Equals(JointOrientation obj) { return(this.JointType.Equals(obj.JointType) && this.Orientation.Equals(obj.Orientation)); }
private static Quaternion GetQuaternion3FromJoint(Kinect.JointOrientation joint) { return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.W)); }
public void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { // Set the position of the avatar based on the kinect body location Kinect.Joint SpineMid = body.Joints[Kinect.JointType.SpineMid]; transform.parent.position = GetVector3FromJoint(SpineMid); // Set the rotation of the avatar based on the kinect body rotation Kinect.JointOrientation SpineShoulderOrientation = body.JointOrientations[Kinect.JointType.SpineShoulder]; RotateAvatar(transform, SpineShoulderOrientation); // Get the position and rotation of both hands from Kinect Kinect.Joint HandRight = body.Joints[Kinect.JointType.HandRight]; Kinect.Joint HandLeft = body.Joints[Kinect.JointType.HandLeft]; Kinect.JointOrientation HandRightOrientation = body.JointOrientations[Kinect.JointType.HandRight]; Kinect.JointOrientation HandLeftOrientation = body.JointOrientations[Kinect.JointType.HandLeft]; // Get the position and rotation of both feet from Kinect Kinect.Joint FootRight = body.Joints[Kinect.JointType.FootRight]; Kinect.Joint FootLeft = body.Joints[Kinect.JointType.FootLeft]; Kinect.JointOrientation FootRightOrientation = body.JointOrientations[Kinect.JointType.AnkleRight]; Kinect.JointOrientation FootLeftOrientation = body.JointOrientations[Kinect.JointType.AnkleLeft]; for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++) { Kinect.Joint sourceJoint = body.Joints[jt]; Kinect.Joint?targetJoint = null; if (_BoneMap.ContainsKey(jt)) { targetJoint = body.Joints[_BoneMap[jt]]; } Transform jointObj = bodyObject.transform.Find(jt.ToString()); jointObj.localPosition = GetVector3FromJoint(sourceJoint); // Map the left and right IK targets to the left and right hand and foot joints in the Kinect model if (jt.ToString().Equals("HandLeft")) { _HandRightTarget.transform.position = jointObj.localPosition; _HandRightTarget.transform.rotation = GetQuaternionFromJointOrientation(HandLeftOrientation); _HandRightTarget.transform.Rotate(HandLeftRotationAdjustment); } if (jt.ToString().Equals("HandRight")) { _HandLeftTarget.transform.position = jointObj.localPosition; _HandLeftTarget.transform.rotation = GetQuaternionFromJointOrientation(HandRightOrientation); _HandLeftTarget.transform.Rotate(HandRightRotationAdjustment); } if (jt.ToString().Equals("FootLeft")) { _FootRightTarget.transform.position = jointObj.localPosition; _FootRightTarget.transform.rotation = GetQuaternionFromJointOrientation(FootLeftOrientation); _FootRightTarget.transform.Rotate(FootLeftRotationAdjustment); } if (jt.ToString().Equals("FootRight")) { _FootLeftTarget.transform.position = jointObj.localPosition; _FootLeftTarget.transform.rotation = GetQuaternionFromJointOrientation(FootRightOrientation); _FootLeftTarget.transform.Rotate(FootRightRotationAdjustment); } if (ShowSkeleton) { LineRenderer lr = jointObj.GetComponent <LineRenderer>(); if (targetJoint.HasValue) { lr.SetPosition(0, jointObj.localPosition); lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value)); lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState)); } else { lr.enabled = false; } } } }
private void RotateAvatar(Transform t, Kinect.JointOrientation jo) { transform.parent.rotation = GetQuaternionFromJointOrientation(jo); transform.parent.Rotate(new Vector3(0f, 180f, 0f)); }
private Vector3 GetEulerAnglesFromJointOrientation(Kinect.JointOrientation jo) { Quaternion joQuaternion = new Quaternion(jo.Orientation.X, jo.Orientation.Y, jo.Orientation.Z, jo.Orientation.W); return(joQuaternion.eulerAngles); }
private Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jo) { Quaternion joQuaternion = new Quaternion(jo.Orientation.X, jo.Orientation.Y, jo.Orientation.Z, jo.Orientation.W); return(joQuaternion); }
public static Quaternion KinectData2Quaternion(Kinect.JointOrientation joint) { return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.Z)); }
public static Quaternion GetQuaternion(Kinect.JointOrientation jointOrientation) { return(GetQuaternion(jointOrientation.Orientation)); }
private static Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jointOrientation) { return(new Quaternion(-jointOrientation.Orientation.X, jointOrientation.Orientation.Y, -jointOrientation.Orientation.Z, jointOrientation.Orientation.W)); }
public SkeletonJoint(Windows.Kinect.Joint joint, JointOrientation orientation) { this.Type = joint.JointType; this.Confidence = (int)joint.TrackingState; this.State = 0; this.Position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); this.Rotation = new Quaternion(orientation.Orientation.X, orientation.Orientation.Y, orientation.Orientation.Z, orientation.Orientation.W); }
public void JointUpdate(Dictionary <Kinect.JointType, Kinect.Joint> joints, Dictionary <Kinect.JointType, Kinect.JointOrientation> orients, int JointID, Kinect.JointType type, Kinect.TrackingState state, TRANSFORM_SMOOTH_PARAMETERS smoothingParams, int body_id = 0) { //printf("body: %d\n", body_id); Vector3 vPrevRawPosition; Vector3 vPrevFilteredPosition; Vector3 vPrevTrend; Vector3 vRawPosition; Vector3 vFilteredPosition; Vector3 vPredictedPosition; Vector3 vDiff; Vector3 vTrend; float vLength; float fDiff; bool bJointIsValid; Quaternion filteredOrientation; Quaternion trend; Quaternion rawOrientation = new Quaternion(orients[(Kinect.JointType)JointID].Orientation.X, orients[(Kinect.JointType)JointID].Orientation.Y, orients[(Kinect.JointType)JointID].Orientation.Z, orients[(Kinect.JointType)JointID].Orientation.W); Quaternion prevFilteredOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation; Quaternion prevTrend = m_pHistory[body_id, JointID].m_vTrendOrientation; Kinect.Joint joint = joints[(Kinect.JointType)JointID]; Kinect.JointOrientation orient = orients[(Kinect.JointType)JointID]; vRawPosition = new Vector4(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f); vPrevFilteredPosition = m_pHistory[body_id, JointID].m_vFilteredPosition; vPrevTrend = m_pHistory[body_id, JointID].m_vTrend; vPrevRawPosition = m_pHistory[body_id, JointID].m_vRawPosition; bJointIsValid = JointPositionIsValid(vRawPosition); // If joint is invalid, reset the filter if (!bJointIsValid) { rawOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation; m_pHistory[body_id, JointID].m_dwFrameCount = 0; } // Initial start values if (m_pHistory[body_id, JointID].m_dwFrameCount == 0) { vFilteredPosition = vRawPosition; vTrend = Vector3.zero; m_pHistory[body_id, JointID].m_dwFrameCount++; filteredOrientation = rawOrientation; trend = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f); } else if (m_pHistory[body_id, JointID].m_dwFrameCount == 1) { vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = (vDiff * smoothingParams.fCorrection) + vPrevTrend * (1.0f - smoothingParams.fCorrection); m_pHistory[body_id, JointID].m_dwFrameCount++; Quaternion prevRawOrientation = m_pHistory[body_id, JointID].m_vRawOrientation; filteredOrientation = EnhansedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = EnhansedQuaternionSlerp(prevTrend, diffStarted, smoothingParams.fCorrection); } else { // First apply jitter filter vDiff = vRawPosition - vPrevFilteredPosition; vLength = Mathf.Sqrt(vDiff.sqrMagnitude); fDiff = Mathf.Abs(vLength); if (fDiff <= smoothingParams.fJitterRadius) { vFilteredPosition = vRawPosition * (fDiff / smoothingParams.fJitterRadius) + vPrevFilteredPosition * (1.0f - fDiff / smoothingParams.fJitterRadius); } else { vFilteredPosition = vRawPosition; } Quaternion diffJitter = RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = Mathf.Abs(QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParams.fJitterRadius) { filteredOrientation = EnhansedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParams.fJitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter vFilteredPosition = vFilteredPosition * (1.0f - smoothingParams.fSmoothing) + (vPrevFilteredPosition + vPrevTrend) * smoothingParams.fSmoothing; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = ((vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection))); // Now the double exponential smoothing filter filteredOrientation = EnhansedQuaternionSlerp(filteredOrientation, (prevFilteredOrientation * prevTrend), smoothingParams.fSmoothing); diffJitter = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = EnhansedQuaternionSlerp(prevTrend, diffJitter, smoothingParams.fCorrection); } // Predict into the future to reduce latency vPredictedPosition = (vFilteredPosition + (vTrend * smoothingParams.fPrediction)); Quaternion predictedOrientation = (filteredOrientation * (EnhansedQuaternionSlerp(new Quaternion(0.0f, 0.0f, 0.0f, 1.0f), trend, smoothingParams.fPrediction))); // Check that we are not too far away from raw data vDiff = (vRawPosition - vPrevFilteredPosition); vLength = Mathf.Sqrt(vDiff.sqrMagnitude); fDiff = Mathf.Abs(vLength); Quaternion diff = RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = Mathf.Abs(QuaternionAngle(diff)); if (fDiff > smoothingParams.fMaxDeviationRadius) { vPredictedPosition = ((vPredictedPosition * (smoothingParams.fMaxDeviationRadius / fDiff)) + (vRawPosition * (1.0f - smoothingParams.fMaxDeviationRadius / fDiff))); } if (diffVal > smoothingParams.fMaxDeviationRadius) { predictedOrientation = EnhansedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParams.fMaxDeviationRadius / diffVal); } predictedOrientation = QuaternionNormalise(predictedOrientation); filteredOrientation = QuaternionNormalise(filteredOrientation); trend = QuaternionNormalise(trend); // Save the data from this frame m_pHistory[body_id, JointID].m_vRawPosition = vRawPosition; m_pHistory[body_id, JointID].m_vFilteredPosition = vFilteredPosition; m_pHistory[body_id, JointID].m_vTrend = vTrend; if (!(rawOrientation == null) && !(filteredOrientation == null) && !(trend == null)) { m_pHistory[body_id, JointID].m_vRawOrientation = rawOrientation; m_pHistory[body_id, JointID].m_vFilteredOrientation = filteredOrientation; m_pHistory[body_id, JointID].m_vTrendOrientation = trend; } // Output the data //vPredictedPosition = new Vector4(vPredictedPosition , 1.0f); Kinect.Joint j = new Windows.Kinect.Joint(); Kinect.CameraSpacePoint CameraSpacePoint = new Windows.Kinect.CameraSpacePoint(); CameraSpacePoint.X = vPredictedPosition.x; CameraSpacePoint.Y = vPredictedPosition.y; CameraSpacePoint.Z = vPredictedPosition.z; j.Position = CameraSpacePoint; j.JointType = type; j.TrackingState = state; m_pFilteredJoints[(Kinect.JointType)JointID] = j; // HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is if (type != Kinect.JointType.SpineBase) { Kinect.JointOrientation jo = new Windows.Kinect.JointOrientation(); Kinect.Vector4 v4 = new Kinect.Vector4(); v4.X = predictedOrientation.x; v4.Y = predictedOrientation.y; v4.Z = predictedOrientation.z; v4.W = predictedOrientation.w; jo.Orientation = v4; jo.JointType = type; m_pFilteredOrientations[(Kinect.JointType)JointID] = jo; } else { m_pFilteredOrientations[(Kinect.JointType)JointID] = orients[(Kinect.JointType)JointID]; } //m_pFilteredJoints[JointID] }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++) { Kinect.Joint sourceJoint = body.Joints[jt]; Kinect.JointOrientation source_ori = body.JointOrientations [jt]; Kinect.Joint? targetJoint = null; if (_BoneMap.ContainsKey(jt)) { targetJoint = body.Joints[_BoneMap[jt]]; } Material zzz_obj = bodyObject.transform.Find(jt.ToString()).gameObject.GetComponent <Renderer> ().material; zzz_obj.color = MyColorPicker.ShowColor; Transform jointObj = bodyObject.transform.Find(jt.ToString()); jointObj.localPosition = GetVector3FromJoint(sourceJoint, body_scale); LineRenderer lr = jointObj.GetComponent <LineRenderer>(); if (targetJoint.HasValue) { lr.SetPosition(0, jointObj.localPosition); lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value, body_scale)); lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState)); } else { lr.enabled = false; } /*if (jt == Kinect.JointType.ElbowLeft) { * L_Elbow.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * L_Elbow.rotation = temp; * } * if (jt == Kinect.JointType.ShoulderLeft) { * L_UpperArm.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * L_UpperArm.rotation = temp; * } * if (jt == Kinect.JointType.WristLeft) { * L_Hand.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * L_Hand.rotation = temp; * } * if (jt == Kinect.JointType.Head) { * Head.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * Head.rotation = temp; * } * if (jt == Kinect.JointType.Neck) { * Neck.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * Neck.rotation = temp; * } * if (jt == Kinect.JointType.SpineShoulder) { * Spineshoulder.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * Spineshoulder.rotation = temp; * } * if (jt == Kinect.JointType.SpineMid) { * SpineMid.position = GetVector3FromJoint(sourceJoint, body_scale); * SpineMid2.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * SpineMid.rotation = temp; * SpineMid2.rotation = temp; * } * if (jt == Kinect.JointType.SpineBase) { * SpineBase.position = GetVector3FromJoint(sourceJoint, body_scale); * SpineBase2.position = GetVector3FromJoint(sourceJoint, body_scale); * Quaternion temp; * temp.w = source_ori.Orientation.W; * temp.x = source_ori.Orientation.X; * temp.y = source_ori.Orientation.Y; * temp.z = source_ori.Orientation.Z; * SpineBase.rotation = temp; * SpineBase2.rotation = temp; * }*/ } }