/// <summary>
        /// Extension method to convert from LeapMotion matrix struct to Wave matrix.
        /// </summary>
        /// <param name="matrix">The Leap motion matrix</param>
        /// <returns>The WaveEngine matrix.</returns>
        public static WaveEngine.Common.Math.Matrix ToMatrix(this Leap.Matrix matrix)
        {
            WaveEngine.Common.Math.Matrix result;
            var array4x4 = matrix.ToArray4x4();

            result.M11 = array4x4[0];
            result.M12 = array4x4[1];
            result.M13 = array4x4[2];
            result.M14 = array4x4[3];

            result.M21 = array4x4[4];
            result.M22 = array4x4[5];
            result.M23 = array4x4[6];
            result.M24 = array4x4[7];

            result.M31 = array4x4[8];
            result.M32 = array4x4[9];
            result.M33 = array4x4[10];
            result.M34 = array4x4[11];

            result.M41 = array4x4[12];
            result.M42 = array4x4[13];
            result.M43 = array4x4[14];
            result.M44 = array4x4[15];

            return(result);
        }
Ejemplo n.º 2
0
        private void ProcessData(Object data)
        {
            string receivedData = (string)data;

            byte[] serialiedFrame = Convert.FromBase64String(receivedData);
            frame.Deserialize(serialiedFrame);
            // frame contains at least one hand
            bool right = false;
            bool left  = false;

            handsValues.Time       = frame.Timestamp;
            handsValues.LeftValues = new float[9] {
                -1f, -1f, -1f, -1f, -1f, -1f, -1f, -1f, -1f
            };
            handsValues.RightValues = new float[9] {
                -1f, -1f, -1f, -1f, -1f, -1f, -1f, -1f, -1f
            };
            foreach (Leap.Hand hand in frame.Hands)
            {
                if (hand.IsValid)
                {
                    float[] anglesArray = new float[9];
                    for (int finger = 0; finger < ftype.Length; finger++)
                    {
                        Leap.Matrix[] basis     = new Leap.Matrix[3];
                        Leap.Vector[] positions = new Leap.Vector[4];

                        for (int bone = 0; bone < btype.Length; bone++)
                        {
                            if (bone < 3)
                            {
                                basis[bone] = hand.Fingers.FingerType(ftype[finger])[0].Bone(btype[bone]).Basis.RigidInverse();
                            }
                            positions[bone] = hand.Fingers.FingerType(ftype[finger])[0].Bone(btype[bone]).NextJoint;
                        }

                        // angleFlex sums 3 flexion angles per finger (metacarpal to proximal - proximal to intermediate - intermediate to distal)
                        float angleFlex = 0;
                        for (int joint = 0; joint < 3; joint++)
                        {
                            angleFlex -= basis[joint].TransformPoint(positions[joint + 1] - positions[joint]).Pitch;
                        }
                        angleFlex           = angleFlex * DEG_TO_RAD;
                        anglesArray[finger] = angleFlex;

                        if (ftype[finger] == Finger.FingerType.TYPE_INDEX || ftype[finger] == Finger.FingerType.TYPE_MIDDLE || ftype[finger] == Finger.FingerType.TYPE_THUMB)
                        {
                            float angleAbd = basis[0].TransformPoint(positions[1] - positions[0]).Yaw *DEG_TO_RAD;
                            anglesArray[finger + 5] = angleAbd;
                        }
                    }

                    anglesArray[8] = -1; // wrist angle

                    float[] vettoreAngoliNorm = normalize(anglesArray);

                    #region leftHand
                    if (hand.IsLeft && !left)
                    {
                        handsValues.LeftValues = vettoreAngoliNorm;
                        left = true;

                        for (int i = 1; i < 10; i++)
                        {
                            string labelName = String.Format("lblLH{0:D1}", i);
                            this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                                   new Action(() => ((Label)this.FindName(labelName)).Content = String.Format("{0:F2}", anglesArray[i - 1])));

                            string sliderName = String.Format("sldLH{0:D1}", i);
                            this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                                   new Action(() => ((Slider)this.FindName(sliderName)).Value = vettoreAngoliNorm[i - 1]));
                        }

                        LHSent += 1;
                        this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                               new Action(() => lblLHSent.Content = LHSent));
                    }
                    #endregion

                    #region rightHand
                    if (hand.IsRight && !right)
                    {
                        handsValues.RightValues = vettoreAngoliNorm;
                        right = true;

                        for (int i = 1; i < 10; i++)
                        {
                            string labelName = String.Format("lblRH{0:D1}", i);
                            this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                                   new Action(() => ((Label)this.FindName(labelName)).Content = String.Format("{0:F2}", anglesArray[i - 1])));

                            string sliderName = String.Format("sldRH{0:D1}", i);
                            this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                                   new Action(() => ((Slider)this.FindName(sliderName)).Value = vettoreAngoliNorm[i - 1]));
                        }

                        RHSent += 1;
                        this.Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Background,
                                               new Action(() => lblRHSent.Content = RHSent));
                    }
                    #endregion

                    #region send to yarp
                    string toSend = ComUtils.XmlUtils.Serialize <LRValues>(handsValues) + " ";
                    yarpPortSend.sendData(toSend);
                    #endregion
                }
            }
        }