private double[,] ComputeTransformation(SerializableBody messageBody, SerializableBody entryBody, Dictionary <Joint, Joint> matching)
{
if (entryBody == null || messageBody == null)
{
if (DEBUG >= 1)
{
Console.WriteLine("Transform unsuccessful - null value");
}
return(null);
}
JointType[] jointTypes = new JointType[] { JointType.Neck, JointType.SpineMid, JointType.ShoulderLeft, JointType.ShoulderRight };
foreach (JointType jointType in jointTypes)
{
if (!matching.ContainsKey(messageBody.Joints[jointType]))
{
matching.Add(messageBody.Joints[jointType], entryBody.Joints[jointType]);
}
}
double[,] matrixCoeff = SolveMatrix(matching);
return(matrixCoeff);
}
public MovingState IsMoving(Body body, SerializableBody lastBody)
{
if (lastBody == null || !body.IsTracked)
{
return(MovingState.Moving);
}
foreach (JointType jointType in importantJoints)
{
if (body.Joints[jointType].TrackingState != TrackingState.Tracked)
{
return(MovingState.Moving);
}
}
float diffSquareDistance = 0;
foreach (KeyValuePair <JointType, Joint> entry in body.Joints)
{
diffSquareDistance += EuclideanSquareDistance(entry.Value, lastBody.Joints[entry.Key]);
}
Console.WriteLine("Moving Diff: " + diffSquareDistance);
if (diffSquareDistance <= MOVING_DISTANCE_EPSILON)
{
return(MovingState.Stand);
}
else
{
return(MovingState.Moving);
}
}
// TODO Set just for gesture body! - Done
private double[,] PrepareTransform(string srcAddress, SerializableBody srcBody, string tgtAddress, SerializableBody tgtBody)
{
double[,] matrixCoeff;
foreach (TopologyEdge edge in Topology)
{
if (edge.SourceIPAddress.Equals(srcAddress) && edge.TargetIPAddress.Equals(tgtAddress))
{
matrixCoeff = this.ComputeTransformation(srcBody, tgtBody, edge.Matching);
edge.TransformMatrixCoefficients = matrixCoeff;
return(matrixCoeff);
}
else if (edge.SourceIPAddress.Equals(tgtAddress) && edge.TargetIPAddress.Equals(srcAddress))
{
matrixCoeff = this.ComputeTransformation(tgtBody, srcBody, edge.Matching);
edge.TransformMatrixCoefficients = matrixCoeff;
return(matrixCoeff);
}
}
Dictionary <Joint, Joint> matching = new Dictionary <Joint, Joint>();
matrixCoeff = this.ComputeTransformation(srcBody, tgtBody, matching);
Topology.Add(new TopologyEdge(srcAddress, tgtAddress, matching, matrixCoeff));
// find graph center
this.FindGraphCenter();
return(matrixCoeff);
}
private void TransformBodies(SerializableBody[] bodies, double[,] matrixCoeff, string sourceAddress)
{
SerializableBody[] transformedBodies = new SerializableBody[bodies.Length];
for (int i = 0; i < transformedBodies.Length; i++)
{
if (bodies[i].IsTracked)
{
transformedBodies[i] = TransformBody(bodies[i], matrixCoeff);
}
}
this.BodiesDictionary.AddOrUpdate(sourceAddress, transformedBodies, (k, v) => transformedBodies);
}
private MessageData PrepareData(Body[] bodies)
{
MessageData message = new MessageData();
message.MessageType = SharedData.Type.Data;
message.SrcIPAddress = sender.LocalEndPoint.ToString();
message.Timestamp = DateTime.Now.Add(timestampDiff);
SerializableBody[] serializableBodies = new SerializableBody[bodies.Length];
for (int i = 0; i < bodies.Length; i++)
{
serializableBodies[i] = PrepareBody(bodies[i], (lastBodies == null || lastBodies.Length <= i) ? null : lastBodies[i]);
}
message.Bodies = serializableBodies;
lastBodies = serializableBodies;
return(message);
}
public SerializableBody TransformBody(SerializableBody body, double[,] coefficients)
{
SerializableBody transformedBody = body.Clone();
Dictionary <JointType, Joint> joints = new Dictionary <JointType, Joint>();
foreach (KeyValuePair <JointType, Joint> entry in body.Joints)
{
joints.Add(entry.Key, TransformJoint(entry.Value, coefficients));
}
IReadOnlyDictionary <JointType, Joint> transformedJoints = joints;
transformedBody.Joints = transformedJoints;
return(transformedBody);
}
private SerializableBody PrepareBody(Body body, SerializableBody lastBody)
{
MovingState movingState = IsMoving(body, lastBody);
CalibrationState calibrationState = CalibrationState.Uncalibrated;
RaisedHand raisedHand = RaisedHand.None;
bool gestureDetected = false;
if (lastBody != null)
{
switch (lastBody.CalibrationState)
{
// raise hand
case CalibrationState.Uncalibrated:
gestureDetected = false;
raisedHand = IsRaisedHand(body);
calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.RaisedHand;
break;
// wait to stand
case CalibrationState.RaisedHand:
if (lastBody.GestureDetected)
{
gestureDetected = false;
calibrationState = CalibrationState.SavedPose;
raisedHand = lastBody.RaisedHand;
break;
}
if (movingState == MovingState.Stand)
{
gestureDetected = true;
calibrationState = CalibrationState.RaisedHand;
raisedHand = lastBody.RaisedHand;
// show notification
this.mainWindow.Dispatcher.Invoke(
new Action(() =>
{
Storyboard sb = this.mainWindow.notificationEllipse.FindResource("notificationStoryBoard") as Storyboard;
sb.Begin();
}));
}
else
{
gestureDetected = false;
raisedHand = IsRaisedHand(body);
calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.RaisedHand;
}
break;
// lower hand
case CalibrationState.SavedPose:
gestureDetected = false;
raisedHand = IsRaisedHand(body);
calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.SavedPose;
break;
}
}
return(new SerializableBody(body, movingState, calibrationState, raisedHand, gestureDetected));
}
