private void Reader_ColorFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Get color data
using (ColorFrame coloredFrame =
reference.ColorFrameReference.AcquireFrame())
{
if (coloredFrame != null)
{
Log.WriteStats(Utility.CalculateFrameRate().ToString(), Log.Stats.FPS);
frameDisplayImage.Source = ToBitmap(coloredFrame);
_processingStage.CompleteProcessingStage(ProcessingStage.Description.KinectStream);
}
}
// filter background with bodyIndex
using (BodyIndexFrame bodyIndexFrame = reference.BodyIndexFrameReference.AcquireFrame())
{
if (bodyIndexFrame != null)
{
var description = bodyIndexFrame.FrameDescription;
byte[] array = new byte[description.Width * description.Height];
bodyIndexFrame.CopyFrameDataToArray(array);
}
}
// display hand tracking
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
var bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(bodies);
foreach (var body in bodies)
{
if (body != null)
{
if (body.IsTracked)
{
_processingStage.CompleteProcessingStage(ProcessingStage.Description.FeatureDetection);
Joint head = body.Joints[JointType.Head];
float x = head.Position.X;
float y = head.Position.Y;
float z = head.Position.Z;
// Find the joints
Joint handRight = body.Joints[JointType.HandRight];
Joint thumbRight = body.Joints[JointType.ThumbRight];
// Tip
Joint tipRight = body.Joints[JointType.HandTipRight];
Joint tipLeft = body.Joints[JointType.HandTipLeft];
Joint handLeft = body.Joints[JointType.HandLeft];
Joint thumbLeft = body.Joints[JointType.ThumbLeft];
// elbow
Joint elbowLeft = body.Joints[JointType.ElbowLeft];
// train the filters
if (tipLeft.TrackingState == TrackingState.Inferred)
{
continue;
}
tipRight.Position = FilterGroup.GetFilter(FilterGroup.Description.FingertipRight).Filter(tipRight.Position);
handRight.Position = FilterGroup.GetFilter(FilterGroup.Description.HandRight).Filter(handRight.Position);
tipLeft.Position = FilterGroup.GetFilter(FilterGroup.Description.Fingertip).Filter(tipLeft.Position);
handLeft.Position = FilterGroup.GetFilter(FilterGroup.Description.Hand).Filter(handLeft.Position);
thumbLeft.Position = FilterGroup.GetFilter(FilterGroup.Description.ThumbTip).Filter(thumbLeft.Position);
elbowLeft.Position = FilterGroup.GetFilter(FilterGroup.Description.Elbow).Filter(elbowLeft.Position);
// canvas ray
var vector = new Vector3D
{
X = tipRight.Position.X - handRight.Position.X,
Y = tipRight.Position.Y - handRight.Position.Y,
Z = tipRight.Position.Z - handRight.Position.Z
};
var tipR = tipRight.Position;
// Refresh Points: standard arm,fingertip and vector
while (_canvas.Children.Count > 3)
{
_canvas.Children.RemoveRange(0, 2);
}
//Util.DrawPoint(canvas, head);
Util.DrawPoint(_canvas, tipRight);
ColorSpacePoint colorPoint = this.sensor.CoordinateMapper.MapCameraPointToColorSpace(handRight.Position);
handRight = Util.ScaleTo(handRight, _canvas.ActualWidth, _canvas.ActualHeight);
thumbRight = Util.ScaleTo(thumbRight, _canvas.ActualWidth, _canvas.ActualHeight);
tipRight = Util.ScaleTo(tipRight, _canvas.ActualWidth, _canvas.ActualHeight);
vector = new Vector3D
{
X = tipRight.Position.X - handRight.Position.X,
Y = tipRight.Position.Y - handRight.Position.Y,
Z = tipRight.Position.Z - handRight.Position.Z
} *10;
var thumbR = thumbRight.Position;
var thumbRPoint3D = new Point3D(thumbR.X, thumbR.Y, thumbR.Z);
tipR = tipRight.Position;
var tipRPoint3D = new Point3D(tipR.X, tipR.Y, tipR.Z);
Util.DrawLine(_canvas, tipRPoint3D, tipRPoint3D + vector, Colors.Violet, 2);
BuildBoundingBoxAroundLeftArm(
Util.ScaleTo(tipLeft, _canvas.ActualWidth, _canvas.ActualHeight),
Util.ScaleTo(elbowLeft, _canvas.ActualWidth, _canvas.ActualHeight));
///////////////////////// RAYTRACING STARTS HERE /////////////////////
if (_bvh != null)
{
Ray ray = new Ray(new Point3D(tipR.X, tipR.Y, tipR.Z), vector);
// for sphere testing, set origin at zero
// Ray ray = new Ray(new Point3D(0,0,0), vector);
Intersection inter = _bvh.Intersect(ray, float.MaxValue);
if (inter._distance > 0) // Found Intersection
{
//AnnotationHandler.AddIntersectedPoint(inter._ray.GetPoint(inter._distance));
Log.WriteLog("detected Hit on Sphere");
_renderer.CreatePointCloud(new Point3DCollection(inter._node._objects), Brushes.OrangeRed, false);
}
}
}
}
}
}
}
}
