public bool SimilarTo(TreeBodyPart bpt2)
{
if (double.IsNaN(x) || double.IsNaN(bpt2.x))
{
return(false);
}
bool result = true;
if (bpt2.at)
{
double distance = Math.Sqrt(Math.Pow(x - bpt2.x, 2) + Math.Pow(y - bpt2.y, 2) + Math.Pow(z - bpt2.z, 2));
result = distance < PRECISION;
}
int i = 0;
foreach (TreeBodyPart bpt in children)
{
result = result && bpt.SimilarTo(bpt2.children[i]);
i = i + 1;
}
return(result);
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
BodyFrameReference frameReference = e.FrameReference;
if (this.startTime.Ticks == 0)
{
this.startTime = frameReference.RelativeTime;
}
try
{
BodyFrame frame = frameReference.AcquireFrame();
if (frame != null)
{
// BodyFrame is IDisposable
using (frame)
{
this.framesSinceUpdate++;
// update status unless last message is sticky for a while
if (DateTime.Now >= this.nextStatusUpdate)
{
// calcuate fps based on last frame received
double fps = 0.0;
if (this.stopwatch.IsRunning)
{
this.stopwatch.Stop();
fps = this.framesSinceUpdate / this.stopwatch.Elapsed.TotalSeconds;
this.stopwatch.Reset();
}
this.nextStatusUpdate = DateTime.Now + TimeSpan.FromSeconds(1);
this.StatusText = string.Format(Properties.Resources.StandardStatusTextFormat, fps, frameReference.RelativeTime - this.startTime);
}
if (!this.stopwatch.IsRunning)
{
this.framesSinceUpdate = 0;
this.stopwatch.Start();
}
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
frame.GetAndRefreshBodyData(this.bodies);
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(joints[jointType].Position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
Dictionary<JointType, Vector3D> bodyVectors = MapJointsToVectors(joints);
bodyPartTree = new TreeBodyPart(bodyVectors, Side.C, BodyPart.SpineMid, Brushes.Firebrick);
////Language Test
//IEnumerable<TreeBodyPart> res;
//res = bodyPartTree.spineshoulder();
//foreach (var b in res)
//{
// bodyPartTree = b;
//}
////res = bodyPartTree.elbow();
////foreach (var b in res)
////{
//// bodyPartTree = b;
////}
//bodyPartTree.ReComputeLabel();
//this.FeedbackText.Visibility = System.Windows.Visibility.Visible;
//if (bodyPartTree.ispointingup())
// this.FeedbackText.Text = "straight";
//else
// this.FeedbackText.Text = "take care";
//res = bodyPartTree.resetjt();
//foreach (var b in res)
//{
// bodyPartTree = b;
//}
if (started && first)
{
first = false;
updated = true;
}
if (updated)
{
//If a skeleton has matched we compute the new set of available moves
if (matched)
{
moves = moveSeq.AvailableMoves();
myPen = new List<Pen>();
shadows = new List<TreeBodyPart>();
shadow = new TreeBodyPart(bodyVectors, Side.C, BodyPart.SpineMid, Brushes.AliceBlue);
//foreach (Transduction m in moves)
//{
shadow = shadow.MakeMove(moves[0]);
while (!shadow.IsActivated() && !moveSeq.isFinal())
{
moveSeq.MakeOneStep(moves[0]);
moves = moveSeq.AvailableMoves();
shadow = shadow.MakeMove(moves[0]);
}
shadows.Add(shadow);
shadow = new TreeBodyPart(bodyVectors, Side.C, BodyPart.SpineMid, Brushes.AliceBlue);
//}
matched = false;
}
for (int i = 0; i < shadows.Count; i++)
{
if (bodyPartTree.SimilarTo(shadows[i]))
{
moveSeq.MakeOneStep(moves[i]);
if (moveSeq.isFinal())
{
updated = false;
started = false;
ActivIn();
}
else
{
matched = true;
}
break;
}
}
}
if (started && updated)
{
foreach (var s in shadows)
{
Dictionary<JointType, Vector3D> vectors = new Dictionary<JointType, Vector3D>();
Dictionary<JointType, bool> actList = new Dictionary<JointType, bool>();
s.GetVectors(ref vectors, ref actList);
//Dictionary<JointType, Joint> shadowJoints = MapVectorsToJoints(vectors, joints);
Dictionary<JointType, Joint> shadowJoints = MapShadowVectorsToJoints(vectors, actList, joints);
Dictionary<JointType, Point> shadowJointPoints = new Dictionary<JointType, Point>();
foreach (JointType shadowJointType in shadowJoints.Keys)
{
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(shadowJoints[shadowJointType].Position);
shadowJointPoints[shadowJointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawShadow(shadowJointPoints, jointPoints, actList, dc);
}
}
// prevent drawing outside of our render area
//this.Dispatcher.Invoke(new Clipp(this.ClippIn), new object[] { });
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
}
catch (Exception)
{
// ignore if the frame is no longer available
}
}
public TreeBodyPart(Dictionary <JointType, Vector3D> vectors3D, Side s, BodyPart bodyPart, Brush b)
{
children = new TreeBodyPart[BodyPartUtil.getRank(bodyPart)];
symbol = bodyPart;
this.s = s;
switch (bodyPart)
{
case BodyPart.SpineMid:
{
HelperJointToXYZ(vectors3D, JointType.SpineMid, ref x, ref y, ref z);
s = Side.C;
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.SpineShoulder, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.SpineBase, b);
break;
}
case BodyPart.SpineShoulder:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.SpineShoulder, ref x, ref y, ref z);
s = Side.C;
children[0] = new TreeBodyPart(vectors3D, Side.L, BodyPart.Shoulder, b);
children[1] = new TreeBodyPart(vectors3D, Side.C, BodyPart.Neck, b);
children[2] = new TreeBodyPart(vectors3D, Side.R, BodyPart.Shoulder, b);
break;
}
}
break;
}
case BodyPart.Shoulder:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ShoulderRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Elbow, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ShoulderLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Elbow, b); break;
}
}
break;
}
case BodyPart.Elbow:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ElbowRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Wrist, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ElbowLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Wrist, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Wrist:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.WristRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Hand, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.WristLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Hand, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Hand:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HandRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.HandTip, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.Thumb, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HandLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.HandTip, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.Thumb, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.HandTip:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HandTipRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HandTipLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Thumb:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ThumbRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ThumbLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Neck:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.Neck, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Head, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Head:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.Head, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.SpineBase:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.SpineBase, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, Side.L, BodyPart.Hip, b);
children[1] = new TreeBodyPart(vectors3D, Side.R, BodyPart.Hip, b);
break;
}
}
break;
}
case BodyPart.Hip:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HipRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Knee, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HipLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Knee, b);
break;
}
}
break;
}
case BodyPart.Knee:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.KneeRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Ankle, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.KneeLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Ankle, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Ankle:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.AnkleRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Foot, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.AnkleLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Foot, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Foot:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.FootRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.FootLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
default: throw new Exception();
}
this.langsLabel = new HashSet <string>();
ComputeLabel();
}
public TreeBodyPart(Dictionary<JointType, Vector3D> vectors3D, Side s, BodyPart bodyPart, Brush b)
{
children = new TreeBodyPart[BodyPartUtil.getRank(bodyPart)];
symbol = bodyPart;
this.s = s;
switch (bodyPart)
{
case BodyPart.SpineMid:
{
HelperJointToXYZ(vectors3D, JointType.SpineMid, ref x, ref y, ref z);
s = Side.C;
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.SpineShoulder, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.SpineBase, b);
break;
}
case BodyPart.SpineShoulder:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.SpineShoulder, ref x, ref y, ref z);
s = Side.C;
children[0] = new TreeBodyPart(vectors3D, Side.L, BodyPart.Shoulder, b);
children[1] = new TreeBodyPart(vectors3D, Side.C, BodyPart.Neck, b);
children[2] = new TreeBodyPart(vectors3D, Side.R, BodyPart.Shoulder, b);
break;
}
} break;
}
case BodyPart.Shoulder:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ShoulderRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Elbow, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ShoulderLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Elbow, b); break;
}
}
break;
}
case BodyPart.Elbow:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ElbowRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Wrist, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ElbowLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Wrist, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Wrist:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.WristRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Hand, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.WristLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Hand, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Hand:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HandRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.HandTip, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.Thumb, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HandLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.HandTip, b);
children[1] = new TreeBodyPart(vectors3D, s, BodyPart.Thumb, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.HandTip:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HandTipRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HandTipLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Thumb:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.ThumbRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.ThumbLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Neck:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.Neck, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Head, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Head:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.Head, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.SpineBase:
{
switch (s)
{
case Side.C:
{
HelperJointToXYZ(vectors3D, JointType.SpineBase, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, Side.L, BodyPart.Hip, b);
children[1] = new TreeBodyPart(vectors3D, Side.R, BodyPart.Hip, b);
break;
}
}
break;
}
case BodyPart.Hip:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.HipRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Knee, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.HipLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Knee, b);
break;
}
}
break;
}
case BodyPart.Knee:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.KneeRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Ankle, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.KneeLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Ankle, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Ankle:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.AnkleRight, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Foot, b);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.AnkleLeft, ref x, ref y, ref z);
children[0] = new TreeBodyPart(vectors3D, s, BodyPart.Foot, b);
break;
}
default: throw new Exception();
}
break;
}
case BodyPart.Foot:
{
switch (s)
{
case Side.R:
{
HelperJointToXYZ(vectors3D, JointType.FootRight, ref x, ref y, ref z);
break;
}
case Side.L:
{
HelperJointToXYZ(vectors3D, JointType.FootLeft, ref x, ref y, ref z);
break;
}
default: throw new Exception();
}
break;
}
default: throw new Exception();
}
this.langsLabel = new HashSet<string>();
ComputeLabel();
}
public bool SimilarTo(TreeBodyPart bpt2)
{
if (double.IsNaN(x) || double.IsNaN(bpt2.x))
{
return false;
}
bool result = true;
if (bpt2.at)
{
double distance = Math.Sqrt(Math.Pow(x - bpt2.x, 2) + Math.Pow(y - bpt2.y, 2) + Math.Pow(z - bpt2.z, 2));
result = distance < PRECISION;
}
int i = 0;
foreach (TreeBodyPart bpt in children)
{
result = result && bpt.SimilarTo(bpt2.children[i]);
i = i + 1;
}
return result;
}
public static TreeBodyPart MakeTree(BodyPart symbol, double x, double y, double z, Side s, bool jt, bool st, bool at, TreeBodyPart[] children)
{
if (children.Length == BodyPartUtil.getRank(symbol))
{
TreeBodyPart t = new TreeBodyPart(symbol, x, y, z, s, jt, st, at, children);
return t;
}
return null;
}
private TreeBodyPart(BodyPart symbol, double x, double y, double z, Side s, bool jt, bool st, bool at, TreeBodyPart[] children)
{
this.symbol = symbol;
this.x = x;
this.y = y;
this.z = z;
this.s = s;
this.jt = jt;
this.st = st;
this.at = at;
this.children = children;
this.langsLabel = new HashSet<string>();
ComputeLabel();
}
