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();
     }
 }
Beispiel #2
0
    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);
        }
    }
Beispiel #3
0
 public bool Equals(JointOrientation obj)
 {
     return JointType.Equals(obj.JointType) && Orientation.Equals(obj.Orientation);
 }
Beispiel #4
0
 // 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));
 }
Beispiel #5
0
 private static Quaternion GetQuaternion3FromJoint(Kinect.JointOrientation joint)
 {
     return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.W));
 }
Beispiel #6
0
    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;
                }
            }
        }
    }
Beispiel #7
0
 private void RotateAvatar(Transform t, Kinect.JointOrientation jo)
 {
     transform.parent.rotation = GetQuaternionFromJointOrientation(jo);
     transform.parent.Rotate(new Vector3(0f, 180f, 0f));
 }
Beispiel #8
0
    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);
    }
Beispiel #9
0
    private Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jo)
    {
        Quaternion joQuaternion = new Quaternion(jo.Orientation.X, jo.Orientation.Y, jo.Orientation.Z, jo.Orientation.W);

        return(joQuaternion);
    }
Beispiel #10
0
 public static Quaternion KinectData2Quaternion(Kinect.JointOrientation joint)
 {
     return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.Z));
 }
Beispiel #11
0
 public static Quaternion GetQuaternion(Kinect.JointOrientation jointOrientation)
 {
     return(GetQuaternion(jointOrientation.Orientation));
 }
Beispiel #12
0
 private static Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jointOrientation)
 {
     return(new Quaternion(-jointOrientation.Orientation.X, jointOrientation.Orientation.Y, -jointOrientation.Orientation.Z, jointOrientation.Orientation.W));
 }
Beispiel #13
0
 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]
    }
Beispiel #15
0
    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;
             * }*/
        }
    }