/// <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);
}
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
}
}
}
