/*
* private const int DMAFilterN = 3;
* private Filter DMAFilterX = new DMAFilter(DMAFilterN);
* private Filter DMAFilterY = new DMAFilter(DMAFilterN);
*
* private const int MedianFilterN = 4;
* private Filter MedianFilterX = new MedianFilter(MedianFilterN);
* private Filter MedianFilterY = new MedianFilter(MedianFilterN);
*/
/// <summary>
/// draw output points on the screen
/// </summary>
private void DrawOutput()
{
using (DrawingContext dc = this.outputDrawingGroup.Open())
{
dc.DrawImage(blankColorBitmap, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
RoomSetting.PaintPlatesAndCoordinates(dc);
foreach (Player player in this.players)
{
player.DrawSkeleton(dc);
if (player.headAndHandValid == true)
{
SpacePoint intersection = RoomSetting.FindTheIntersection(RoomSetting.CameraPointToRoomPoint(player.startPointInCameraCoordinates),
RoomSetting.CameraPointToRoomPoint(player.endPointInCameraCoordinates));
if (intersection != null)
{
Point showPoint = RoomSetting.RoomPointToObservePoint(intersection);
dc.DrawLine(new Pen(player.color, 2), RoomSetting.CameraPointToObservePoint(player.startPointInCameraCoordinates),
showPoint);
if (showPoint.X >= 0 && showPoint.X < RenderWidth && showPoint.Y >= 0 && showPoint.Y < RenderHeight)
{
dc.DrawEllipse(player.color, null, showPoint, this.PointThickness, this.PointThickness);
}
else
{
RenderClippedEdges(showPoint, dc);
}
}
}
}
}
}
/// <summary>
/// Map point from camera coordinates to room coordinates
/// </summary>
public static SpacePoint CameraPointToRoomPoint(SpacePoint cameraPoint)
{
double depth = cameraPoint.Z;
cameraPoint.Z = 1;
SpacePoint cameraCoordinatesPoint = new SpacePoint(CameraMatrixInverse * (depth * cameraPoint));
// 对cameraCoordinatesPoint进行旋转平移得到roompoint
SpacePoint roomPoint = new SpacePoint(Kinect2RoomRotation * cameraCoordinatesPoint + Kinect2RoomTranslation);
//System.Diagnostics.Debug.WriteLine(roomPoint.X.ToString() + " " + roomPoint.Y.ToString() + " " + roomPoint.Z.ToString());
return(roomPoint);
}
/// <summary>
/// Fine the intersection on the room's walls
/// </summary>
public static SpacePoint FindTheIntersection(SpacePoint start, SpacePoint end)
{
foreach (Plate plate in roomPlates)
{
if (plate != null)
{
SpacePoint normal = plate.NormalVector;
double t = -((start.X - plate.A.X) * normal.X + (start.Y - plate.A.Y) * normal.Y + (start.Z - plate.A.Z) * normal.Z)
/ ((end - start).X * normal.X + (end - start).Y * normal.Y + (end - start).Z * normal.Z);
SpacePoint intersection = start + t * (end - start);
//System.Diagnostics.Debug.WriteLine(intersection.X);
//System.Diagnostics.Debug.WriteLine(intersection.X.ToString() + " " + intersection.Y.ToString() + " " + intersection.Z.ToString());
if (t > 0 && plate.InsidePlate(intersection))
{
intersectionFilter.SetValue(intersection);
intersection = intersectionFilter.GetValue();
plate.Active = true;
if (plate == roomPlates[0])
{
if (move == true)
{
SpacePoint translation = intersection - 0.5 * (plate.A + plate.C);
plate.A += translation;
plate.B += translation;
plate.C += translation;
plate.D += translation;
plate.Holding = true;
}
else
{
plate.Holding = false;
}
}
return(intersection);
}
}
}
return(null);
}
/// <summary>
/// Map point from room coordinates to observe window
/// </summary>
public static Point RoomPointToObservePoint(SpacePoint roomPoint)
{
// 对 roomPoint 进行旋转平移得到 observe matrix
SpacePoint ObservePoint = new SpacePoint(Room2ObserveRotation * (roomPoint - Room2ObserveTranslation));
//System.Diagnostics.Debug.WriteLine(roomPoint.X.ToString() + " " + roomPoint.Y.ToString() + " " + roomPoint.Z.ToString());
double depth = ObservePoint.Z;
SpacePoint ObserveScreenPoint = new SpacePoint((1 / depth) * (ObserveMatrix * ObservePoint));
//System.Diagnostics.Debug.WriteLine(roomPoint.X.ToString() + " " + roomPoint.Y.ToString() + " " + roomPoint.Z.ToString());
//System.Diagnostics.Debug.WriteLine(ObservePoint.X.ToString() + " " + ObservePoint.Y.ToString() + " " + ObservePoint.Z.ToString());
//System.Diagnostics.Debug.WriteLine(ObserveScreenPoint.X.ToString() + " " + ObserveScreenPoint.Y.ToString() + " " + ObserveScreenPoint.Z.ToString());
return(new Point(ObserveScreenPoint.X, ObserveScreenPoint.Y));
}
public bool InsidePlate(SpacePoint point)
{
SpacePoint A = this.A;
SpacePoint B = this.B;
SpacePoint C = this.C;
SpacePoint D = this.D;
if (Holding == true)
{
A = this.A + (500 / SpacePoint.Norm(this.A - this.C)) * (this.A - this.C);
B = this.B + (500 / SpacePoint.Norm(this.B - this.D)) * (this.B - this.D);
C = this.C + (500 / SpacePoint.Norm(this.C - this.A)) * (this.C - this.A);
D = this.D + (500 / SpacePoint.Norm(this.D - this.B)) * (this.D - this.B);
}
return(SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(B - A, point - A)) * SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(D - A, point - A)) < 0 &&
SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(A - B, point - B)) * SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(C - B, point - B)) < 0 &&
SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(B - C, point - C)) * SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(D - C, point - C)) < 0);
}
/// <summary>
/// Called to update point info
/// </summary>
public void AnalyzeHeadAndHands()
{
if (skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked)
{
//System.Diagnostics.Debug.WriteLine(RoomSetting.CameraPointToRoomPoint(this.painter.SkeletonPointToCameraPoint(skeleton.Joints[JointType.HandLeft].Position)).Z);
if (RoomSetting.CameraPointToRoomPoint(this.painter.SkeletonPointToCameraPoint(skeleton.Joints[JointType.HandLeft].Position)).Z > 1200)
{
RoomSetting.move = true;
}
else
{
RoomSetting.move = false;
}
}
// If we can't find either head or right hand, exit
if (skeleton.Joints[JointType.Head].TrackingState == JointTrackingState.NotTracked ||
skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.NotTracked)
{
this.headAndHandValid = false;
return;
}
// Don't analyze if both points are inferred
if (skeleton.Joints[JointType.Head].TrackingState == JointTrackingState.Inferred &&
skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Inferred)
{
this.headAndHandValid = false;
return;
}
this.headAndHandValid = true;
//this.startPointInColorFrame = this.painter.SkeletonPointToScreen(skeleton.Joints[JointType.Head].Position);
this.startPointInCameraCoordinates = this.painter.SkeletonPointToCameraPoint(skeleton.Joints[JointType.Head].Position);
//this.endPointInColorFrame = this.painter.SkeletonPointToScreen(skeleton.Joints[JointType.HandRight].Position);
this.endPointInCameraCoordinates = this.painter.SkeletonPointToCameraPoint(skeleton.Joints[JointType.HandRight].Position);
}
public Plate(SpacePoint cA, SpacePoint cB, SpacePoint cC, SpacePoint cD)
{
A = cA;
B = cB;
C = cC;
D = cD;
// ABC三点共线或ABCD四点不在同一平面上
if (SpacePoint.Norm(SpacePoint.CrossProduct(B - A, C - A)) < 1e-6 ||
System.Math.Abs(SpacePoint.DotProduct(SpacePoint.CrossProduct(B - A, C - A), D - A)) > 1e-6)
{
System.Diagnostics.Debug.WriteLine("!!!Wall ERROR!!!!");
}
NormalVector = SpacePoint.CrossProduct(B - A, D - A);
Active = false;
if (SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(B - A, C - A)) * SpacePoint.DotProduct(NormalVector, SpacePoint.CrossProduct(D - A, C - A)) > 0)
{
SpacePoint t = C;
C = D;
D = t;
}
}
public static double Norm(SpacePoint point)
{
return(System.Math.Sqrt(point.X * point.X + point.Y * point.Y + point.Z * point.Z));
}
public static double DotProduct(SpacePoint left, SpacePoint right)
{
return(left.X * right.X + left.Y * right.Y + left.Z * right.Z);
}
public static SpacePoint CrossProduct(SpacePoint left, SpacePoint right)
{
return(new SpacePoint(left.Y * right.Z - left.Z * right.Y, left.Z * right.X - left.X * right.Z, left.X * right.Y - left.Y * right.X));
}
public void SetValue(SpacePoint newData)
{
FilterX.SetValue(newData.X);
FilterY.SetValue(newData.Y);
FilterZ.SetValue(newData.Z);
}
public static Point CameraPointToObservePoint(SpacePoint cameraPoint)
{
return(RoomPointToObservePoint(CameraPointToRoomPoint(cameraPoint)));
}
