Example #1
0
    void TransformBone(uint userId, SkeletonJoint joint, Transform dest, bool move)
    {
        SkeletonJointPosition sjp = this.joints[userId][joint];
        Point3D pos = sjp.position;
        SkeletonJointOrientation ori = new SkeletonJointOrientation();

        this.skeletonCapbility.GetSkeletonJointOrientation(userId, joint, ori);
        float [] m = ori.Orientation.elements;
        // only modify joint if confidence is high enough in this frame
        if (ori.Confidence > 0.5)
        {
            // Z coordinate in OpenNI is opposite from Unity. We will create a quat
            // to rotate from OpenNI to Unity (relative to initial rotation)
            //    Vector3 worldZVec = new Vector3(-ori.m02, -ori.m12, ori.m22);
            Vector3 worldZVec = new Vector3(-m[2], -m[5], m[8]);

            //   Vector3 worldYVec = new Vector3(trans.ori.m01, trans.ori.m11, -trans.ori.m21);
            Vector3 worldYVec = new Vector3(m[1], m[4], -m[7]);

            Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);

            Quaternion newRotation = jointRotation * initialRotations[(int)joint];

            // Some smoothing
            dest.rotation = Quaternion.Slerp(dest.rotation, newRotation, Time.deltaTime * 20);
        }

        if (move)
        {
//			dest.position = new Vector3(trans.pos.x/1000, trans.pos.y/1000 -1, -trans.pos.z/1000);
            dest.position = new Vector3(pos.X / 1000, pos.Y / 1000 - 1, -pos.Z / 1000);
        }
    }
Example #2
0
    public void JointFromJSON(SkeletonJoint j, Hashtable dict)
    {
        ArrayList positionList = (ArrayList)dict["Position"];

        ArrayList orientationList    = (ArrayList)dict["Orientation"];
        SkeletonJointOrientation sjo = new SkeletonJointOrientation();

        sjo.X1 = 1.0f;
        SkeletonJointPosition       sjp   = new SkeletonJointPosition();
        SkeletonJointTransformation xform = new SkeletonJointTransformation();

        // object -> double ->float is okay, but object->float isn't
        // (the object is a Double)
        sjp.Position = new Point3D((float)(double)positionList[0],
                                   (float)(double)positionList[1],
                                   (float)(double)positionList[2]);
        sjo.X1            = (float)(double)orientationList[0];
        sjo.X2            = (float)(double)orientationList[1];
        sjo.X3            = (float)(double)orientationList[2];
        sjo.Y1            = (float)(double)orientationList[3];
        sjo.Y2            = (float)(double)orientationList[4];
        sjo.Y3            = (float)(double)orientationList[5];
        sjo.Z1            = (float)(double)orientationList[6];
        sjo.Z2            = (float)(double)orientationList[7];
        sjo.Z3            = (float)(double)orientationList[8];
        xform.Orientation = sjo;
        xform.Position    = sjp;
        UpdateJoint(j, xform);
    }
Example #3
0
        public SkeletonJointOrientation GetSkeletonJointOrientation(UserID user, SkeletonJoint joint)
        {
            SkeletonJointOrientation orientation = new SkeletonJointOrientation();
            int status = SafeNativeMethods.xnGetSkeletonJointOrientation(this.InternalObject, user, joint, ref orientation);

            WrapperUtils.ThrowOnError(status);
            return(orientation);
        }
Example #4
0
    public static bool GetJointOrientation(uint userID, int joint, ref SkeletonJointOrientation pOrientation)
    {
        if (GetJointOrientationNative(userID, joint, ref pOrientation))
        {
            return(true);
        }

        return(false);
    }
Example #5
0
	/// initialize from OpenNI data
	public void FromRaw(SkeletonJointTransformation k)
	{
//		m_joint = joint;
		Point3D p = k.Position.Position;
		m_pos.x = p.X;
		m_pos.y = p.Y;
		m_pos.z = 1500-p.Z; // the z axis of the (Kinect) sensor is opposite to the one in Unity3D. 
		// 1500 is a arbitrary offset to make the value around 0.
		m_posConf = k.Position.Confidence;
		SkeletonJointOrientation o = k.Orientation;
		m_orient = new Vector3[3];
		m_orient[0].x = o.X1; m_orient[0].y = o.X2; m_orient[0].z = -o.X3; // the z axis of the sensor is opposite to the one in Unity3D
		m_orient[1].x = o.Y1; m_orient[1].y = o.Y2; m_orient[1].z = -o.Y3;
		m_orient[2].x = o.Z1; m_orient[2].y = o.Z2; m_orient[2].z = -o.Z3;
		m_orientConf = o.Confidence;
	}
Example #6
0
    void TransformBone(uint userId, SkeletonJoint joint, Transform dest, bool move)
    {
        //   SkeletonJointPosition sjp = this.joints[userId][joint];
        //   Point3D pos = sjp.position;

        //Debug.Log("joint " + joint + "x " + pos.X + " y " + pos.Y + " z " + pos.Z);

        SkeletonJointOrientation ori = new SkeletonJointOrientation();

        this.skeletonCapbility.GetSkeletonJointOrientation(userId, joint, ref ori);
        //float [] m = ori.Orientation.elements;

        //Debug.Log(m.Length);


        // only modify joint if confidence is high enough in this frame
        if (ori.Confidence > 0.5)
        {
            // Z coordinate in OpenNI is opposite from Unity. We will create a quat
            // to rotate from OpenNI to Unity (relative to initial rotation)
            //    Vector3 worldZVec = new Vector3(-ori.m02, -ori.m12, ori.m22);
            //  elem1 elem2 elem3
            //  elem4 elem5 elem6
            //  elem7 elem8 elem9
            Vector3 worldZVec = new Vector3(-ori.Orientation.elem3, -ori.Orientation.elem6, ori.Orientation.elem9);

            //   Vector3 worldYVec = new Vector3(trans.ori.m01, trans.ori.m11, -trans.ori.m21);
            Vector3 worldYVec = new Vector3(ori.Orientation.elem2, ori.Orientation.elem5, -ori.Orientation.elem8);

            Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);

            Quaternion newRotation = (jointRotation * initialRotations[(int)joint]);

            // Some smoothing
            dest.rotation = Quaternion.Slerp(dest.rotation, newRotation, Time.deltaTime * 20);
        }

        //if (move)
        //{
//			dest.position = new Vector3(trans.pos.x/1000, trans.pos.y/1000 -1, -trans.pos.z/1000);
        //	dest.position = new Vector3(pos.X/1000, pos.Y/1000 -1, -pos.Z/1000);


        //}
    }
Example #7
0
    public static Quaternion SkeletonJointOrientationToQuaternion(SkeletonJointOrientation m)
    {
        float tr = m.X1 + m.Y2 + m.Z3;

        float S  = 0f;
        float qw = 0f;
        float qx = 0f;
        float qy = 0f;
        float qz = 0f;

        if (tr > 0)
        {
            S  = Mathf.Sqrt(tr + 1.0f) * 2f;
            qw = 0.25f * S;
            qx = (m.Y3 - m.Z2) / S;
            qy = (m.Z1 - m.X3) / S;
            qz = (m.X2 - m.Y1) / S;
        }
        else if ((m.X1 > m.Y2) && (m.X1 > m.Z3))
        {
            S  = Mathf.Sqrt(1.0f + m.X1 - m.Y2 - m.Z3) * 2f;
            qw = (m.Y3 - m.Z2) / S;
            qx = 0.25f * S;
            qy = (m.Y1 + m.X2) / S;
            qz = (m.Z1 + m.X3) / S;
        }
        else if (m.Y2 > m.Z3)
        {
            S  = Mathf.Sqrt(1.0f + m.Y2 - m.X1 - m.Z3) * 2f;
            qw = (m.Z1 - m.X3) / S;
            qx = (m.Y1 + m.X2) / S;
            qy = 0.25f * S;
            qz = (m.Z2 + m.Y3) / S;
        }
        else
        {
            S  = Mathf.Sqrt(1.0f + m.Z3 - m.X1 - m.Y2) * 2f;
            qw = (m.X2 - m.Y1) / S;
            qx = (m.Z1 + m.X3) / S;
            qy = (m.Z2 + m.Y3) / S;
            qz = 0.25f * S;
        }
        return(new Quaternion(qx, qy, qz, qw));
    }
 /// @brief Gets the current joint orientation for a specific joint
 ///
 /// @param joint The joint we want the orientation to.
 /// @param curRotation [out] The current joint rotation
 /// @return True on success and false on failure (e.g. the user is not tracking).
 /// @note An exception might occur if there is an error (e.g. an illegal joint is used).
 public bool GetSkeletonJointOrientation(SkeletonJoint joint, out SkeletonJointOrientation curRotation)
 {
     curRotation = NIPlayerCandidateObject.m_InitializedZero.Orientation;
     if (!Tracking)
     {
         return(false);
     }
     if (m_user.Skeleton == null)
     {
         return(false);
     }
     try
     {
         curRotation = m_user.Skeleton.GetSkeletonJointOrientation(m_user.OpenNIUserID, joint);
     }
     catch
     {
         return(false);
     }
     return(true);
 }
Example #9
0
        public void DrawOrientation(ref WriteableBitmap image, int id, UserGenerator userGenerator, SkeletonJoint joint, Point3D corner)
        {
            SkeletonJointOrientation orientation = new SkeletonJointOrientation();
            SkeletonJointPosition    position    = new SkeletonJointPosition();

            position    = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, joint);
            orientation = userGenerator.SkeletonCapability.GetSkeletonJointOrientation(id, joint);

            if (position.Confidence != 1 && orientation.Confidence != 1)
            {
                return;
            }

            SkeletonJointPosition v1 = new SkeletonJointPosition();
            SkeletonJointPosition v2 = new SkeletonJointPosition();

            v1.Confidence = v2.Confidence = 1;

            v1.Position = position.Position;
            v2.Position = new Point3D(v1.Position.X + 100 * orientation.X1,
                                      v1.Position.Y + 100 * orientation.Y1,
                                      v1.Position.Z + 100 * orientation.Z1);

            DrawTheLine(ref image, ref v1, ref v2);

            v1.Position = position.Position;
            v2.Position = new Point3D(v1.Position.X + 100 * orientation.X2,
                                      v1.Position.Y + 100 * orientation.Y2,
                                      v1.Position.Z + 100 * orientation.Z2);

            DrawTheLine(ref image, ref v1, ref v2);

            v1.Position = position.Position;
            v2.Position = new Point3D(v1.Position.X + 100 * orientation.X3,
                                      v1.Position.Y + 100 * orientation.Y3,
                                      v1.Position.Z + 100 * orientation.Z3);

            DrawTheLine(ref image, ref v1, ref v2);
        }
Example #10
0
    // Update is called once per frame
    void Update()
    {
        //Debug.Log("Update");
        if (this.shouldRun)
        {
            try{
                this.context.WaitOneUpdateAll(this.depth);
            }catch (Exception) {
                Debug.Log("No paso");
            }
            //Debug.Log("H "+Input.GetAxis("Horizontal"));
            //Debug.Log("V "+Input.GetAxis("Vertical"));

            int[] users = this.userGenerator.GetUsers();
            foreach (int user in users)
            {
                if (this.skeletonCapability.IsTracking(user))
                {
                    //Debug.Log ("Esta trackeando Usuario # "+user);
                    //obtener la orientacion del joint
                    SkeletonJointOrientation ori = this.skeletonCapability.GetSkeletonJointOrientation(user, SkeletonJoint.Torso);

                    /*
                     * Debug.Log("X1= "+ori.X1);
                     * Debug.Log("X2= "+ori.X2);
                     * Debug.Log("X3= "+ori.X3);
                     * Debug.Log("Y1= "+ori.Y1);
                     * Debug.Log("Y2= "+ori.Y2);
                     * Debug.Log("Y3= "+ori.Y3);
                     * Debug.Log("Z1= "+ori.Z1);
                     * Debug.Log("Z2= "+ori.Z2);
                     * Debug.Log("Z3= "+ori.Z3);
                     */
                    Quaternion q = SkeletonJointOrientationToQuaternion(ori);
                    //Debug.Log(q.y);
                    if (q.y > .30)
                    {
                        transform.Rotate(new Vector3(0f, valorPositivoRotacion, 0f));
                    }
                    else if (q.y < -.30)
                    {
                        transform.Rotate(new Vector3(0f, -valorNegativoRotacion, 0f));
                    }



                    if (q.x > .20)
                    {
                        if (transform.rotation.x < .25)
                        {
                            transform.Rotate(new Vector3(valorPositivoRotacion, 0f, 0f));
                        }
                    }
                    else if (q.x < -.20)
                    {
                        if (transform.rotation.x > -.25)
                        {
                            transform.Rotate(new Vector3(-valorNegativoRotacion, 0f, 0f));
                        }
                    }
                    else
                    {
                        Quaternion fromX = new Quaternion(transform.rotation.x, transform.rotation.y, 0f, transform.rotation.w);
                        Quaternion toX   = new Quaternion(0f, transform.rotation.y, 0f, transform.rotation.w);


                        transform.rotation = Quaternion.Lerp(fromX, toX, Time.time * .001f);
                        //transform.Rotate (new Vector3((transform.rotation.x *-1),0f,0f));
                    }
                }
            }
        }
    }
Example #11
0
    public static bool GetSkeletonJointOrientation(uint userID, int joint, bool flip, ref Quaternion pJointRot)
    {
        Matrix4x4 matOri     = Matrix4x4.identity;
        bool      bMatHasOri = false;

        if (joint == (int)SkeletonJoint.LEFT_HAND)
        {
            // special case - left hand
            SkeletonJointPosition posElbow = new SkeletonJointPosition();
            SkeletonJointPosition posHand  = new SkeletonJointPosition();
            SkeletonJointPosition posHips  = new SkeletonJointPosition();
            SkeletonJointPosition posNeck  = new SkeletonJointPosition();

            bool bElbowTracked = GetJointPosition(userID, (int)SkeletonJoint.LEFT_ELBOW, ref posElbow);
            bool bHandTracked  = GetJointPosition(userID, (int)SkeletonJoint.LEFT_HAND, ref posHand);
            bool bHipsTracked  = GetJointPosition(userID, (int)SkeletonJoint.HIPS, ref posHips);
            bool bNeckTracked  = GetJointPosition(userID, (int)SkeletonJoint.NECK, ref posNeck);

            if (bElbowTracked && bHandTracked && bHipsTracked && bNeckTracked)
            {
                Vector3 vElbow = new Vector3(posElbow.x, posElbow.y, posElbow.z);
                Vector3 vHand  = new Vector3(posHand.x, posHand.y, posHand.z);
                Vector3 vHips  = new Vector3(posHips.x, posHips.y, posHips.z);
                Vector3 vNeck  = new Vector3(posNeck.x, posNeck.y, posNeck.z);

                Vector3 vx = -(vHand - vElbow);
                Vector3 vy = vNeck - vHips;

                MakeMatrixFromXY(vx, vy, ref matOri);
                bMatHasOri = true;
            }
        }
        else if (joint == (int)SkeletonJoint.RIGHT_HAND)
        {
            // special case - right hand
            SkeletonJointPosition posElbow = new SkeletonJointPosition();
            SkeletonJointPosition posHand  = new SkeletonJointPosition();
            SkeletonJointPosition posHips  = new SkeletonJointPosition();
            SkeletonJointPosition posNeck  = new SkeletonJointPosition();

            bool bElbowTracked = GetJointPosition(userID, (int)SkeletonJoint.RIGHT_ELBOW, ref posElbow);
            bool bHandTracked  = GetJointPosition(userID, (int)SkeletonJoint.RIGHT_HAND, ref posHand);
            bool bHipsTracked  = GetJointPosition(userID, (int)SkeletonJoint.HIPS, ref posHips);
            bool bNeckTracked  = GetJointPosition(userID, (int)SkeletonJoint.NECK, ref posNeck);

            if (bElbowTracked && bHandTracked && bHipsTracked && bNeckTracked)
            {
                Vector3 vElbow = new Vector3(posElbow.x, posElbow.y, posElbow.z);
                Vector3 vHand  = new Vector3(posHand.x, posHand.y, posHand.z);
                Vector3 vHips  = new Vector3(posHips.x, posHips.y, posHips.z);
                Vector3 vNeck  = new Vector3(posNeck.x, posNeck.y, posNeck.z);

                Vector3 vx = vHand - vElbow;
                Vector3 vy = vNeck - vHips;

                MakeMatrixFromXY(vx, vy, ref matOri);
                bMatHasOri = true;
            }
        }
        else
        {
            // all other joints
            SkeletonJointOrientation oriJoint = new SkeletonJointOrientation();

            if (GetJointOrientation(userID, joint, ref oriJoint))
            {
                Quaternion rotJoint = new Quaternion(oriJoint.x, oriJoint.y, oriJoint.z, oriJoint.w);
                matOri.SetTRS(Vector3.zero, rotJoint, Vector3.one);
                bMatHasOri = true;
            }
        }

        if (bMatHasOri)
        {
            Vector4 vZ = matOri.GetColumn(2);
            Vector4 vY = matOri.GetColumn(1);

            if (!flip)
            {
                vZ.y = -vZ.y;
                vY.x = -vY.x;
                vY.z = -vY.z;
            }
            else
            {
                vZ.x = -vZ.x;
                vZ.y = -vZ.y;
                vY.z = -vY.z;
            }

            if (vZ.x != 0.0f || vZ.y != 0.0f || vZ.z != 0.0f)
            {
                pJointRot = Quaternion.LookRotation(vZ, vY);
            }
            else
            {
                bMatHasOri = false;
            }
        }

        return(bMatHasOri);
    }
Example #12
0
 public static extern bool GetJointOrientationNative(uint userID, int joint, ref SkeletonJointOrientation pOrientation);
Example #13
0
    void Update()
    {
        //Debug.Log("Update");
        if (this.shouldRun)
        {
            try{
                this.context.WaitOneUpdateAll(this.depth);
            }catch (Exception) {
                Debug.Log("No paso");
            }
            //Debug.Log("H "+Input.GetAxis("Horizontal"));
            //Debug.Log("V "+Input.GetAxis("Vertical"));

            int[] users = this.userGenerator.GetUsers();
            foreach (int user in users)
            {
                if (this.skeletonCapability.IsTracking(user))
                {
                    //Debug.Log ("Esta trackeando Usuario # "+user);
                    //obtener la orientacion del joint
                    SkeletonJointOrientation ori = this.skeletonCapability.GetSkeletonJointOrientation(user, SkeletonJoint.Torso);

                    Quaternion q = SkeletonJointOrientationToQuaternion(ori);
                    //Debug.Log(q.y);
                    if (q.y > .30)
                    {
                        transform.Rotate(new Vector3(0f, valorPositivoRotacion, 0f));
                    }
                    else if (q.y < -.30)
                    {
                        transform.Rotate(new Vector3(0f, -valorNegativoRotacion, 0f));
                    }

                    if (q.x > .20)
                    {
                        if (GameObject.Find("CuboCamara").transform.rotation.x < .25)
                        {
                            GameObject.Find("CuboCamara").transform.Rotate(new Vector3(valorPositivoRotacion, 0f, 0f));
                        }
                    }
                    else if (q.x < -.20)
                    {
                        if (GameObject.Find("CuboCamara").transform.rotation.x > -.25)
                        {
                            GameObject.Find("CuboCamara").transform.Rotate(new Vector3(-valorNegativoRotacion, 0f, 0f));
                        }
                    }
                    else
                    {
                        Quaternion fromX = new Quaternion(GameObject.Find("CuboCamara").transform.rotation.x, 0f, 0f, GameObject.Find("CuboCamara").transform.rotation.w);
                        Quaternion toX   = new Quaternion(0f, GameObject.Find("CuboCamara").transform.rotation.y, 0f, transform.rotation.w);


                        //GameObject.Find("CuboCamara").transform.Rotate(Quaternion.Lerp(fromX, toX, Time.time * .001f));
                        //GameObject.Find("CuboCamara").transform.Rotate(Vector3((GameObject.Find("CuboCamara").rotation.x *-1)*2,0f,0f));
                    }

                    //transform.rotation=q;

                    //Traslacion
                    SkeletonJointPosition posicion = this.skeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.Torso);
                    if (posicion.Position.Z < 1170F)
                    {
                        //transform.Translate(Vector3.forward*10f*Time.deltaTime);
                        transform.Translate(Vector3.forward * 0.107832f * 5 * Time.deltaTime);
                    }
                    else if (posicion.Position.Z > 1470F)
                    {
                        //transform.Translate(Vector3.back*10f*Time.deltaTime);
                        transform.Translate(Vector3.back * 0.107832f * 5 * Time.deltaTime);
                    }
                }
            }
        }
    }
Example #14
0
 public void GetSkeletonJointOrientation(UserID user, SkeletonJoint eJoint, SkeletonJointOrientation joint)
 {
     UInt32 status = OpenNIImporter.xnGetSkeletonJointOrientation(this.InternalObject, user, eJoint, joint);
     WrapperUtils.CheckStatus(status);
 }