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
}
}
}
private void btnRead_Click(object sender, RoutedEventArgs e)
{
// Create OpenFileDialog
Microsoft.Win32.OpenFileDialog dlg = new Microsoft.Win32.OpenFileDialog();
// Set filter for file extension and default file extension
dlg.DefaultExt = ".data";
dlg.Filter = "Frame data (*.data)|*.data";
// Display OpenFileDialog by calling ShowDialog method
Nullable<bool> result = dlg.ShowDialog();
// Get the selected file name and display in a TextBox
if (result == true)
{
frameList.Clear();
// Open document
string filename = dlg.FileName;
using (System.IO.BinaryReader br =
new System.IO.BinaryReader(System.IO.File.Open(filename, System.IO.FileMode.Open)))
{
while (br.BaseStream.Position < br.BaseStream.Length)
{
//deserialises and reads the binary file
DateTime date = new DateTime();
date = DateTime.FromBinary(br.ReadInt64());
// Console.WriteLine(date.ToString());
Int32 stimCode = br.ReadInt32();
Int32 nextBlock = br.ReadInt32();
byte[] frameData = br.ReadBytes(nextBlock);
Leap.Frame newFrame = new Leap.Frame();
newFrame.Deserialize(frameData);
if (stimCode == 5443)
{
Debug.WriteLine("5443 detected: " + newFrame.Id);
}
frameList.Add(newFrame);
//Console.WriteLine(newFrame.CurrentFramesPerSecond);
}
br.Close();
br.Dispose();
}
/* WRITE CODE HERE TO EXTRACT DATA FROM FILE
foreach (Leap.Frame frame in frameList)
{
//Console.WriteLine(frame.Id);
}
*/
}
}
