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