public static List <Location> ListAllPossibleLocation(RectangleOriented rectangle) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); PointD corner_1 = corners.Item1; PointD corner_2 = corners.Item2; PointD corner_3 = corners.Item3; PointD corner_4 = corners.Item4; bool farthest_point_is_1 = Toolbox.ConvertPointDToPolar(corner_1).Distance > Toolbox.ConvertPointDToPolar(corner_2).Distance; bool corner_1_and_2_is_height = Math.Round(Toolbox.Distance(corner_1, corner_2)) == ConstVar.HEIGHT_BOXSIZE; // Math.Abs(rotation_angle) > Math.Acos(ConstVar.HEIGHT_BOXSIZE / Math.Sqrt(Math.Pow(ConstVar.HEIGHT_BOXSIZE, 2) + Math.Pow(ConstVar.WIDTH_BOXSIZE, 2))) double rotation_angle = Toolbox.ModuloPiAngleRadian(rectangle.Angle); Matrix <double> rotation_matrix = DenseMatrix.OfArray(new double[, ] { { Math.Cos(rotation_angle), -Math.Sin(rotation_angle) }, { Math.Sin(rotation_angle), Math.Cos(rotation_angle) } }); Vector <double> ref_center = Vector <double> .Build.DenseOfArray(new double[] { rectangle.Center.X, rectangle.Center.Y }) * rotation_matrix; PointD ref_center_point = new PointD(ref_center[0], ref_center[1]); Vector <double> ref_pt1 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item1.X, corners.Item1.Y }) * rotation_matrix; Vector <double> ref_pt2 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item2.X, corners.Item2.Y }) * rotation_matrix; Vector <double> ref_pt3 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item3.X, corners.Item3.Y }) * rotation_matrix; Vector <double> ref_pt4 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item4.X, corners.Item4.Y }) * rotation_matrix; PointD pt1 = new PointD(0, 0); Console.WriteLine(farthest_point_is_1 + " " + corner_1_and_2_is_height + " " + (rotation_angle * 180 / Math.PI)); PointD ref_robot_point = new PointD(-ref_center[0], -ref_center[1]); if (rotation_angle > 0 && corner_1_and_2_is_height) { rotation_angle *= -1; } Location location_1 = new Location(pt1.X - ref_center_point.X, pt1.Y - ref_center_point.Y, -rotation_angle, 0, 0, 0); Location location_4 = new Location(pt1.X + ref_center_point.X, pt1.Y + ref_center_point.Y, rotation_angle, 0, 0, 0); return(new List <Location>() { location_1, location_4 }); }
public static List <SegmentExtended> DrawRectangle(RectangleOriented rectangle, Color color, double width = 3) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); List <SegmentExtended> list_of_segments = new List <SegmentExtended>() { new SegmentExtended(corners.Item1, corners.Item2, color, width), new SegmentExtended(corners.Item1, corners.Item3, color, width), new SegmentExtended(corners.Item4, corners.Item2, color, width), new SegmentExtended(corners.Item4, corners.Item3, color, width), }; return(list_of_segments); }
public static RectangleOriented FindMbrBoxByArea(List <PointD> list_of_points) { if (list_of_points.Count <= 1) { return(new RectangleOriented(new PointD(0, 0), 0, 0, 0)); } ConvexHullCreationResult <DefaultVertex2D> hull = ConvexHull.Create2D(list_of_points.Select(x => new double[2] { x.X, x.Y }).ToList()); double[,] hull_array = new double[hull.Result.Count, 2]; for (int i = 0; i < hull.Result.Count; i++) { hull_array[i, 0] = hull.Result[i].X; hull_array[i, 1] = hull.Result[i].Y; } Matrix <double> hull_matrix = DenseMatrix.OfArray(hull_array); List <double> edge_angles = Rotating_Caliper(hull_array); double? min_area = null; RectangleOriented best_rectangle = new RectangleOriented(); /// Test each angle to find bounding box with smallest area foreach (double angle in edge_angles) { RectangleOriented rectangle = FindMinimumBoundingRectangle(hull_matrix, angle); double area = rectangle.Width * rectangle.Lenght; if (area < min_area || min_area == null) { min_area = area; best_rectangle = rectangle; } } return(best_rectangle); }
public static RectangleOriented FindMbrBoxByOverlap(List <PointD> list_of_points) { ConvexHullCreationResult <DefaultVertex2D> hull = ConvexHull.Create2D(list_of_points.Select(x => new double[2] { x.X, x.Y }).ToList()); double[,] hull_array = new double[hull.Result.Count, 2]; for (int i = 0; i < hull.Result.Count; i++) { hull_array[i, 0] = hull.Result[i].X; hull_array[i, 1] = hull.Result[i].Y; } Matrix <double> hull_matrix = DenseMatrix.OfArray(hull_array); List <double> edge_angles = Rotating_Caliper(hull_array); int?max_overlap = null; RectangleOriented best_rectangle = new RectangleOriented(); /// Test each angle to find bounding box with smallest area foreach (double angle in edge_angles) { RectangleOriented rectangle = FindMinimumBoundingRectangle(hull_matrix, angle); int overlap = FindAllBorderPoints(list_of_points, rectangle, 0.05).Count; if (overlap > max_overlap || max_overlap == null) { max_overlap = overlap; best_rectangle = rectangle; } } return(best_rectangle); }
public void ProcessLidarData(List <PolarPointRssi> polarPointRssi) { List <SegmentExtended> Lines = new List <SegmentExtended>(); List <PolarPointRssi> validPoint = polarPointRssi.Where(x => x.Distance <= Math.Sqrt(Math.Pow(3, 2) + Math.Pow(2, 2))).ToList(); List <PointD> validPointXY = validPoint.Select(x => Toolbox.ConvertPolarToPointD(x)).ToList(); List <PointDExtended> absolutePoints = new List <PointDExtended>(); validPointXY = ClustersDetection.ExtractClusterByDBScan(validPointXY, 0.05, 3).SelectMany(x => x.points).ToList().Select(x => Toolbox.ConvertPolarToPointD(x)).ToList().Select(x => x.Pt).ToList(); RectangleOriented best_rectangle = FindRectangle.FindMbrBoxByOverlap(validPointXY); List <PointD> border_points = FindRectangle.FindAllBorderPoints(validPointXY, best_rectangle, 0.05); List <PolarPointRssi> border_point_polar = border_points.Select(x => Toolbox.ConvertPointDToPolar(x)).ToList(); List <ClusterObjects> inside_clusters = ClustersDetection.ExtractClusterByDBScan(validPointXY.Where(x => border_points.IndexOf(x) == -1).ToList(), 0.045, 3); List <PolarPointRssiExtended> processedPoints = ClustersDetection.SetColorsOfClustersObjects(inside_clusters); List <ClusterObjects> border_clusters = ClustersDetection.ExtractClusterByDBScan(border_points, 0.05, 10); List <SegmentExtended> iepfs_lines = new List <SegmentExtended>(); foreach (ClusterObjects c in border_clusters) { var iepf_border_points = LineDetection.IEPF_Algorithm(c.points.Select(x => Toolbox.ConvertPolarToPointD(x).Pt).ToList(), 0.05); for (int i = 1; i < iepf_border_points.Count; i++) { iepfs_lines.Add(new SegmentExtended(iepf_border_points[i - 1], iepf_border_points[i], Color.Black, 2)); } } iepfs_lines = LineDetection.MergeSegmentWithLSM(iepfs_lines, 0.5, 3 * Math.PI / 180); Lines.AddRange(iepfs_lines); List <List <SegmentExtended> > list_of_family = LineDetection.FindFamilyOfSegment(iepfs_lines); List <PointD> corners_points = CornerDetection.FindAllValidCrossingPoints(list_of_family).SelectMany(x => x).ToList().Select(x => x.Pt).ToList(); Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(best_rectangle); double thresold = 0.09; double width = Math.Max(best_rectangle.Lenght, best_rectangle.Width); double height = Math.Min(best_rectangle.Lenght, best_rectangle.Width); List <SegmentExtended> rectangle_segments = FindRectangle.DrawRectangle(best_rectangle, Color.Green, 1); //if (width >= 3 - thresold && height >= 2 - thresold) //{ // Console.ForegroundColor = ConsoleColor.Green; //} //else if (width >= 3 - thresold) //{ // Console.ForegroundColor = ConsoleColor.Blue; //} //else if (width > 2 + thresold) //{ // Console.ForegroundColor = ConsoleColor.Yellow; // if (height > 2 - thresold) // Console.ForegroundColor = ConsoleColor.DarkYellow; //} //else if (width >= 2 - thresold && width <= 2 + thresold) // Console.ForegroundColor = ConsoleColor.Red; //else // Console.ResetColor(); //Console.WriteLine(width + " " + height); processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(best_rectangle.Center), 10, Color.Black)); Lines.AddRange(rectangle_segments); //Console.WriteLine("Corners: " + FindRectangle.GetNumberOfVisibleCorners(best_rectangle)); RectangleOriented resized_rectangle = FindRectangle.ResizeRectangle(best_rectangle, thresold); List <Location> list_of_possible_locations; if (resized_rectangle != null) { Console.ForegroundColor = ConsoleColor.Green; Lines.AddRange(FindRectangle.DrawRectangle(resized_rectangle, Color.LightGreen, 8)); processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(resized_rectangle.Center), 10, Color.Green)); list_of_possible_locations = FindRectangle.ListAllPossibleLocation(resized_rectangle); //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList()); } else { Tuple <RectangleOriented, RectangleOriented, RectangleOriented> list_of_possible_rectangles = FindRectangle.ListResisableRectangle(best_rectangle, thresold); Console.ForegroundColor = ConsoleColor.Yellow; list_of_possible_locations = FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2); //Console.ForegroundColor = ConsoleColor.Blue; //list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2)); //Console.ForegroundColor = ConsoleColor.Red; //list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item3)); //Console.ForegroundColor = ConsoleColor.Red; //Console.WriteLine("1 ------"); //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList()); //Console.ForegroundColor = ConsoleColor.Yellow; //Console.WriteLine("2 ------"); Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item1, Color.Yellow, 4)); //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item1.Center), 10, Color.Yellow)); Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item2, Color.Blue, 4)); //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item2.Center), 10, Color.Blue)); Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item3, Color.Red, 4)); //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item3.Center), 10, Color.LightGoldenrodYellow)); } //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red: Color.DarkBlue)).ToList()); absolutePoints = list_of_possible_locations.Select(x => new PointDExtended(new PointD(x.X, x.Y), x.Theta > 0 ? Color.Red : Color.Blue, 10)).ToList(); Location best_location = FindRectangle.GetBestLocation(list_of_possible_locations, robotLocation); //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(best_location.X, best_location.Y)), 10, Color.DarkGreen)); OnLidarSetupRobotLocationEvent?.Invoke(this, best_location); Console.ResetColor(); List <Cup> list_of_cups = new List <Cup>(); List <LidarObjects> list_of_objects = new List <LidarObjects>(); foreach (ClusterObjects c in inside_clusters) { RectangleOriented cluster_rectangle = FindRectangle.FindMbrBoxByArea(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList()); cluster_rectangle.Width += 0.1; cluster_rectangle.Lenght += 0.1; //Lines.AddRange(FindRectangle.DrawRectangle(cluster_rectangle, c.points[0].Color, 3)); Color color = Toolbox.ColorFromHSL((list_of_objects.Count * 0.20) % 1, 1, 0.5); list_of_objects.Add(new LidarObjects(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList(), color)); Cup cup = DetectCup(c); // The null condition is Bad need to edit if (cup != null) { list_of_cups.Add(cup); } } OnProcessLidarObjectsDataEvent?.Invoke(this, list_of_objects); // Lines.Add(DetectGlobalLine(polarPointRssi, 1d, 0d, 5d, 3, 0.2d)); OnProcessLidarLineDataEvent?.Invoke(this, Lines); OnProcessLidarCupDataEvent?.Invoke(this, list_of_cups); RawLidarArgs processLidar = new RawLidarArgs() { RobotId = robotId, LidarFrameNumber = LidarFrame, PtList = processedPoints.Select(x => x.Pt).ToList() }; OnProcessLidarDataEvent?.Invoke(this, processLidar); OnProcessLidarPolarDataEvent?.Invoke(this, processedPoints); OnProcessLidarAbsoluteDataEvent?.Invoke(this, absolutePoints); //OnProcessLidarXYDataEvent?.Invoke(this, processedPoints.Select(x => new PointDExtended(Toolbox.ConvertPolarToPointD(x), Color.Blue, 2)).ToList()); }
public static List <PointD> FindAllBorderPoints(List <PointD> list_of_points, RectangleOriented rectangle, double thresold) { double[,] point_array = new double[list_of_points.Count, 2]; for (int i = 0; i < list_of_points.Count; i++) { point_array[i, 0] = list_of_points[i].X; point_array[i, 1] = list_of_points[i].Y; } Matrix <double> points_matrix = DenseMatrix.OfArray(point_array); double angle = rectangle.Angle; Matrix <double> rotation_matrix = DenseMatrix.OfArray(new double[, ] { { Math.Cos(angle), Math.Cos(angle - Math.PI / 2) }, { Math.Cos(angle + Math.PI / 2), Math.Cos(angle) } }); Matrix <double> rotated_points = rotation_matrix * points_matrix.Transpose(); Vector <double> center_point = Vector <double> .Build.DenseOfArray(new double[] { rectangle.Center.X, rectangle.Center.Y }); Vector <double> rotated_center = rotation_matrix * center_point; double min_x = rotated_center[0] - rectangle.Width / 2; double max_x = rotated_center[0] + rectangle.Width / 2; double min_y = rotated_center[1] - rectangle.Lenght / 2; double max_y = rotated_center[1] + rectangle.Lenght / 2; bool[] list_of_rotated_border_points = new bool[rotated_points.ColumnCount]; for (int i = 0; i < rotated_points.ColumnCount; i++) { double current_x = rotated_points[0, i]; double current_y = rotated_points[1, i]; if (Math.Abs(min_x - current_x) <= thresold || Math.Abs(max_x - current_x) <= thresold) { list_of_rotated_border_points[i] = true; } if (Math.Abs(min_y - current_y) <= thresold || Math.Abs(max_y - current_y) <= thresold) { list_of_rotated_border_points[i] = true; } } List <PointD> list_of_border_points = new List <PointD>(); for (int i = 0; i < list_of_points.Count; i++) { if (list_of_rotated_border_points[i]) { list_of_border_points.Add(list_of_points[i]); } } return(list_of_border_points); }
public static List <RectangleOriented> FindAllPossibleRectangle(List <List <PointDExtended> > list_of_family_corners, double thresold) { List <RectangleOriented> list_of_rectangles = new List <RectangleOriented>(); foreach (List <PointDExtended> family in list_of_family_corners) { List <int> list_of_case = Enumerable.Range(0, family.Count).ToList(); /// [0,1,2,3,...,n] List <List <int> > list_of_combinations_of_corner_index = Toolbox.GetKCombs(list_of_case, 2).ToList().Select(x => x.ToList()).ToList(); /// [[0,1],[0,2],[0,3],[1,2],[1,3],[2,3],...] /// [[Dist_0-1, Corner0, Corner1],[Dist_0-2, Corner0,Corner2],[Dist_0-3, Corner0,Corner3],[Dist_1-2, Corner1,Corner2],etc...] List <Tuple <double, PointDExtended, PointDExtended> > list_of_combinations_corners_and_distance = list_of_combinations_of_corner_index.Select( x => new Tuple <double, PointDExtended, PointDExtended>( Toolbox.Distance(family[x[0]].Pt, family[x[1]].Pt), family[x[0]], family[x[1]] ) ).ToList(); double previous_distance = 0; PointD previous_vector_point_a = new PointD(0, 0); PointD previous_vector_point_b = new PointD(0, 0); PointD previous_center_vector_point = new PointD(0, 0); /// Now we sort the list by distance (Greater -> Smaller) foreach (var vector_distance in list_of_combinations_corners_and_distance.OrderByDescending(x => x.Item1)) { PointD actual_vector_point_a = vector_distance.Item2.Pt; PointD actual_vector_point_b = vector_distance.Item3.Pt; PointD actual_center_vector_point = new PointD((actual_vector_point_b.X + actual_vector_point_a.X) / 2, (actual_vector_point_b.Y + actual_vector_point_a.Y) / 2); /// We check if all points are not similar if (Toolbox.Distance(actual_vector_point_a, previous_vector_point_a) != 0 && Toolbox.Distance(actual_vector_point_a, previous_vector_point_b) != 0 && Toolbox.Distance(actual_vector_point_b, previous_vector_point_a) != 0 && Toolbox.Distance(actual_vector_point_b, previous_vector_point_b) != 0) { /// We calculate the center of the two vector /// and if the two center points are close enought whe have a rectangle if (previous_distance + thresold >= vector_distance.Item1 && previous_distance - thresold <= vector_distance.Item1) { if (Toolbox.Distance(actual_center_vector_point, previous_center_vector_point) < thresold) { PointD mean_center_point = new PointD((actual_center_vector_point.X + previous_center_vector_point.X) / 2, (actual_center_vector_point.Y + previous_center_vector_point.Y) / 2); double lenght = Toolbox.Distance(actual_vector_point_a, previous_vector_point_a); double width = Toolbox.Distance(actual_vector_point_b, previous_vector_point_a); double angle = Math.Atan2(actual_vector_point_a.Y - previous_vector_point_a.Y, actual_vector_point_a.X - previous_vector_point_a.X); RectangleOriented rectangle = new RectangleOriented(mean_center_point, lenght, width, angle); list_of_rectangles.Add(rectangle); } } } previous_distance = vector_distance.Item1; previous_vector_point_a = actual_vector_point_a; previous_vector_point_b = actual_vector_point_b; previous_center_vector_point = actual_center_vector_point; } } return(list_of_rectangles); }
public static List <PointD> FindValidCorners(RectangleOriented rectangle, List <PointD> list_of_points, double thresold, double min_points) { List <PointD> list_of_valid_corners = new List <PointD>(); Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); double angle = Math.Abs(Toolbox.ModuloPiAngleRadian(rectangle.Angle)); double[,] point_array = new double[list_of_points.Count, 2]; for (int i = 0; i < list_of_points.Count; i++) { point_array[i, 0] = list_of_points[i].X; point_array[i, 1] = list_of_points[i].Y; } Matrix <double> points_matrix = DenseMatrix.OfArray(point_array); Matrix <double> rotation_matrix = DenseMatrix.OfArray(new double[, ] { { Math.Cos(angle), Math.Cos(angle - Math.PI / 2) }, { Math.Cos(angle + Math.PI / 2), Math.Cos(angle) } }); Matrix <double> rotated_points = rotation_matrix * points_matrix.Transpose(); Vector <double> rotated_center = rotation_matrix * Vector <double> .Build.DenseOfArray(new double[] { rectangle.Center.X, rectangle.Center.Y }); double min_x = rotated_center[0] - rectangle.Width / 2; double max_x = rotated_center[0] + rectangle.Width / 2; double min_y = rotated_center[1] - rectangle.Lenght / 2; double max_y = rotated_center[1] + rectangle.Lenght / 2; int right_border = 0, left_border = 0, top_border = 0, bottom_border = 0; for (int i = 0; i < rotated_points.ColumnCount; i++) { double current_x = rotated_points[0, i]; double current_y = rotated_points[1, i]; if (Math.Abs(min_x - current_x) <= thresold) { left_border++; } if (Math.Abs(max_x - current_x) <= thresold) { right_border++; } if (Math.Abs(min_y - current_y) <= thresold) { top_border++; } if (Math.Abs(max_y - current_y) <= thresold) { bottom_border++; } } if (top_border >= min_points && right_border >= min_points) { list_of_valid_corners.Add(corners.Item2); } if (bottom_border >= min_points && right_border >= min_points) { list_of_valid_corners.Add(corners.Item4); } if (top_border >= min_points && left_border >= min_points) { list_of_valid_corners.Add(corners.Item1); } if (bottom_border >= min_points && left_border >= min_points) { list_of_valid_corners.Add(corners.Item4); } return(list_of_valid_corners); }
public static List <Location> ListAllPossibleLocation(RectangleOriented rectangle) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); PointD corner_1 = corners.Item1; PointD corner_2 = corners.Item2; PointD corner_3 = corners.Item3; PointD corner_4 = corners.Item4; bool farthest_point_is_1 = corner_1.X > corner_2.X; bool corner_1_and_2_is_height = Toolbox.Distance(corner_1, corner_2) == ConstVar.HEIGHT_BOXSIZE; double rotation_angle = Toolbox.ModuloPiAngleRadian(rectangle.Angle); Matrix <double> rotation_matrix = DenseMatrix.OfArray(new double[, ] { { Math.Cos(rotation_angle), -Math.Sin(rotation_angle) }, { Math.Sin(rotation_angle), Math.Cos(rotation_angle) } }); Vector <double> ref_center = Vector <double> .Build.DenseOfArray(new double[] { rectangle.Center.X, rectangle.Center.Y }) * rotation_matrix; PointD ref_robot_point = new PointD(-ref_center[0], -ref_center[1]); Vector <double> ref_pt1 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item1.X, corners.Item1.Y }) * rotation_matrix; Vector <double> ref_pt2 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item2.X, corners.Item2.Y }) * rotation_matrix; Vector <double> ref_pt3 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item3.X, corners.Item3.Y }) * rotation_matrix; Vector <double> ref_pt4 = Vector <double> .Build.DenseOfArray(new double[] { corners.Item4.X, corners.Item4.Y }) * rotation_matrix; //PointD pt1 = new PointD(ref_robot_point.X, ref_robot_point.Y); //PointD pt2 = new PointD(- ref_robot_point.X, - ref_robot_point.Y); PointD pt1 = new PointD(ref_pt1[0], ref_pt1[1]); PointD pt2 = new PointD(-ref_pt1[0], -ref_pt1[1]); if (Math.Abs(rotation_angle) > Math.Acos(ConstVar.HEIGHT_BOXSIZE / Math.Sqrt(Math.Pow(ConstVar.HEIGHT_BOXSIZE, 2) + Math.Pow(ConstVar.WIDTH_BOXSIZE, 2)))) { Toolbox.SwapNum(ref pt1, ref pt2); } Location location_1 = new Location(pt1.X - ref_robot_point.X, pt1.Y - ref_robot_point.Y, -rotation_angle, 0, 0, 0); Location location_2 = new Location(pt2.X - ref_robot_point.X, pt2.Y - ref_robot_point.Y, rotation_angle, 0, 0, 0); if (location_1.X < 0) { Console.WriteLine("Angle X: " + rotation_angle * 180 / Math.PI); } //Location location_1 = new Location(pt1.X - ref_center[0], pt1.Y - ref_center[1], 1, 0, 0, 0); //Location location_2 = new Location(pt2.X - ref_center[0], pt2.Y - ref_center[1], 0, 0, 0, 0); //Location location_3 = new Location(pt3.X - ref_center[0], pt3.Y - ref_center[1], 0, 0, 0, 0); //Location location_4 = new Location(pt4.X - ref_center[0], pt4.Y - ref_center[1], 0, 0, 0, 0); //if (location_1.X >= 0 && location_1.Y >= 0) // Console.WriteLine("Juste: " + rectangle.Angle * 180 / Math.PI); //else if (location_1.X < 0 && location_1.Y >= 0) // Console.WriteLine("Faux X: "); // + rectangle.Angle * 180 / Math.PI); //else if (location_1.X >= 0 && location_1.Y < 0) // Console.WriteLine("Faux Y: "); // + rectangle.Angle * 180 / Math.PI); //else // Console.WriteLine("Tout Faux: " + rectangle.Angle * 180 / Math.PI); return(new List <Location>() { location_1, location_2 }); }
public static RectangleOriented ResizeRectangle(RectangleOriented rectangle, double thresold) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); PointD point_1 = corners.Item1; PointD point_2 = corners.Item2; PointD point_3 = corners.Item3; PointD point_4 = corners.Item4; double Width = Math.Max(rectangle.Lenght, rectangle.Width); double Height = Math.Min(rectangle.Lenght, rectangle.Width); double Angle = rectangle.Angle; PointD correct_center_point = new PointD(0, 0); if (Width >= ConstVar.WIDTH_BOXSIZE - thresold) { if (Height >= ConstVar.HEIGHT_BOXSIZE - thresold) { /// Whe have the full box correct_center_point = rectangle.Center; } else { /// We resize the Height of the box double correction_angle; double correction_distance = (ConstVar.HEIGHT_BOXSIZE - Height) / 2; if (Toolbox.Distance(point_1, point_2) > ConstVar.HEIGHT_BOXSIZE - thresold) { correction_angle = Toolbox.Angle(point_1, point_2) - Math.PI / 2; } else { correction_angle = Toolbox.Angle(point_1, point_2); } if (Toolbox.ModuloPiAngleRadian(correction_angle) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(correction_angle) > -Math.PI / 2) { correction_angle = Toolbox.ModuloPiAngleRadian(correction_angle) + Math.PI; } PointD correction_point = Toolbox.ConvertPolarToPointD(new PolarPointRssi(correction_angle, correction_distance, 0)); correct_center_point = new PointD(rectangle.Center.X + correction_point.X, rectangle.Center.Y + correction_point.Y); } } else if (Width >= ConstVar.HEIGHT_BOXSIZE + thresold) { if (Height >= ConstVar.HEIGHT_BOXSIZE - thresold) { /// Whe only resize the Width double correction_angle; double correction_distance = (ConstVar.WIDTH_BOXSIZE - Width) / 2; if (Toolbox.Distance(point_1, point_2) <= ConstVar.HEIGHT_BOXSIZE + thresold) { correction_angle = Toolbox.Angle(point_1, point_2) - Math.PI / 2; } else { correction_angle = Toolbox.Angle(point_1, point_2); } if (Toolbox.ModuloPiAngleRadian(correction_angle) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(correction_angle) > -Math.PI / 2) { correction_angle = Toolbox.ModuloPiAngleRadian(correction_angle) + Math.PI; } PointD correction_point = Toolbox.ConvertPolarToPointD(new PolarPointRssi(correction_angle, correction_distance, 0)); correct_center_point = new PointD(rectangle.Center.X + correction_point.X, rectangle.Center.Y + correction_point.Y); } else { /// Whe resize the Width double width_correction_angle, height_correction_angle; double width_correction_distance = (ConstVar.WIDTH_BOXSIZE - Width) / 2; double height_correction_distance = (ConstVar.HEIGHT_BOXSIZE - Height) / 2; if (Toolbox.Distance(point_1, point_2) <= ConstVar.HEIGHT_BOXSIZE + thresold) { width_correction_angle = Toolbox.Angle(point_1, point_2) - Math.PI / 2; height_correction_angle = Toolbox.Angle(point_1, point_2); } else { width_correction_angle = Toolbox.Angle(point_1, point_2); height_correction_angle = Toolbox.Angle(point_1, point_2) - Math.PI / 2; } if (Toolbox.ModuloPiAngleRadian(width_correction_angle) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(width_correction_angle) > -Math.PI / 2) { width_correction_angle = Toolbox.ModuloPiAngleRadian(width_correction_angle) + Math.PI; } if (Toolbox.ModuloPiAngleRadian(height_correction_angle) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(height_correction_angle) > -Math.PI / 2) { height_correction_angle = Toolbox.ModuloPiAngleRadian(height_correction_angle) + Math.PI; } PointD width_correction_point = Toolbox.ConvertPolarToPointD(new PolarPointRssi(width_correction_angle, width_correction_distance, 0)); PointD height_correction_point = Toolbox.ConvertPolarToPointD(new PolarPointRssi(height_correction_angle, height_correction_distance, 0)); correct_center_point = new PointD(rectangle.Center.X + width_correction_point.X + height_correction_point.X, rectangle.Center.Y + width_correction_point.Y + height_correction_point.Y); } } else { if (Width >= ConstVar.HEIGHT_BOXSIZE - thresold && Width <= ConstVar.HEIGHT_BOXSIZE + thresold) { return(null); // Not Safe Need Improvement if (Toolbox.Distance(point_1, point_2) < ConstVar.HEIGHT_BOXSIZE - thresold || Toolbox.Distance(point_1, point_2) > ConstVar.HEIGHT_BOXSIZE + thresold) // Not Good -> False Positive + False Negative { return(null); } /// Whe only resize the Width double correction_angle = Toolbox.Angle(point_1, point_2) + Math.PI / 2; double correction_distance = (ConstVar.WIDTH_BOXSIZE - Height) / 2; if (Toolbox.ModuloPiAngleRadian(correction_angle) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(correction_angle) > -Math.PI / 2) { correction_angle = Toolbox.ModuloPiAngleRadian(correction_angle) + Math.PI; } PointD correction_point = Toolbox.ConvertPolarToPointD(new PolarPointRssi(correction_angle, correction_distance, 0)); Angle += Math.PI / 2; correct_center_point = new PointD(rectangle.Center.X + correction_point.X, rectangle.Center.Y + correction_point.Y); } else { return(null); } } return(new RectangleOriented(correct_center_point, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle)); }
public static Tuple <RectangleOriented, RectangleOriented> ListResisableRectangle(RectangleOriented rectangle, double thresold) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); double width_correction_angle_1, height_correction_angle_1, width_correction_angle_2, height_correction_angle_2, width_correction_distance_1, height_correction_distance_1, width_correction_distance_2, height_correction_distance_2; PointD point_1 = corners.Item1; PointD point_2 = corners.Item2; PointD point_3 = corners.Item3; PointD point_4 = corners.Item4; double Width = Math.Max(rectangle.Lenght, rectangle.Width); double Height = Math.Min(rectangle.Lenght, rectangle.Width); double Angle = rectangle.Angle; width_correction_distance_1 = (ConstVar.WIDTH_BOXSIZE - Width) / 2; height_correction_distance_1 = (ConstVar.HEIGHT_BOXSIZE - Height) / 2; width_correction_distance_2 = (ConstVar.WIDTH_BOXSIZE - Height) / 2; height_correction_distance_2 = (ConstVar.HEIGHT_BOXSIZE - Width) / 2; /// 1st width_correction_angle_1 = Angle; height_correction_angle_1 = Angle - Math.PI / 2; if (Toolbox.ModuloPiAngleRadian(width_correction_angle_1) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(width_correction_angle_1) > -Math.PI / 2) { width_correction_angle_1 = Toolbox.ModuloPiAngleRadian(width_correction_angle_1) + Math.PI; } if (Toolbox.ModuloPiAngleRadian(height_correction_angle_1) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(height_correction_angle_1) > -Math.PI / 2) { height_correction_angle_1 = Toolbox.ModuloPiAngleRadian(height_correction_angle_1) + Math.PI; } PointD width_correction_point_1 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(width_correction_angle_1, width_correction_distance_1, 0)); PointD height_correction_point_1 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(height_correction_angle_1, height_correction_distance_1, 0)); PointD correct_center_point_1 = new PointD(rectangle.Center.X + width_correction_point_1.X + height_correction_point_1.X, rectangle.Center.Y + width_correction_point_1.Y + height_correction_point_1.Y); /// 2st width_correction_angle_2 = Angle - Math.PI / 2; height_correction_angle_2 = Angle;; if (Toolbox.ModuloPiAngleRadian(width_correction_angle_2) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(width_correction_angle_2) > -Math.PI / 2) { width_correction_angle_2 = Toolbox.ModuloPiAngleRadian(width_correction_angle_2) + Math.PI; } if (Toolbox.ModuloPiAngleRadian(height_correction_angle_2) < Math.PI / 2 && Toolbox.ModuloPiAngleRadian(height_correction_angle_2) > -Math.PI / 2) { height_correction_angle_2 = Toolbox.ModuloPiAngleRadian(height_correction_angle_2) + Math.PI; } PointD width_correction_point_2 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(width_correction_angle_2, width_correction_distance_2, 0)); PointD height_correction_point_2 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(height_correction_angle_2, height_correction_distance_2, 0)); PointD correct_center_point_2 = new PointD(rectangle.Center.X + width_correction_point_2.X + height_correction_point_2.X, rectangle.Center.Y + width_correction_point_2.Y + height_correction_point_2.Y); RectangleOriented rectangle1 = new RectangleOriented(correct_center_point_1, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle); RectangleOriented rectangle2 = new RectangleOriented(correct_center_point_2, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle + Math.PI / 2); return(new Tuple <RectangleOriented, RectangleOriented>(rectangle1, rectangle2)); }
public static Tuple <RectangleOriented, RectangleOriented, RectangleOriented> ListResisableRectangle(RectangleOriented rectangle, double thresold) { Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(rectangle); double width_correction_angle_1, height_correction_angle_1, width_correction_angle_2, height_correction_angle_2, width_correction_distance_1, height_correction_distance_1, width_correction_distance_2, height_correction_distance_2; PointD point_1 = corners.Item1; PointD point_2 = corners.Item2; PointD point_3 = corners.Item3; PointD point_4 = corners.Item4; double Width = Math.Max(rectangle.Lenght, rectangle.Width); double Height = Math.Min(rectangle.Lenght, rectangle.Width); double Angle = rectangle.Angle; width_correction_distance_1 = (ConstVar.WIDTH_BOXSIZE - Width) / 2; height_correction_distance_1 = (ConstVar.HEIGHT_BOXSIZE - Height) / 2; width_correction_distance_2 = (ConstVar.WIDTH_BOXSIZE - Height) / 2; height_correction_distance_2 = (ConstVar.HEIGHT_BOXSIZE - Width) / 2; if (Width >= ConstVar.HEIGHT_BOXSIZE - thresold) { //width_correction_distance_1 = 0; height_correction_distance_2 = 0; } if (Height >= ConstVar.HEIGHT_BOXSIZE - thresold) { height_correction_distance_1 = 0; //width_correction_distance_2 = 0; } /// 1st width_correction_angle_1 = Toolbox.ModuloPiAngleRadian(Angle) + Math.PI; height_correction_angle_1 = Toolbox.ModuloPiAngleRadian(Angle - Math.PI / 2) + Math.PI; PointD width_correction_point_1 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(width_correction_angle_1, width_correction_distance_1, 0)); PointD height_correction_point_1 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(height_correction_angle_1, height_correction_distance_1, 0)); PointD correct_center_point_1 = new PointD(rectangle.Center.X + width_correction_point_1.X + height_correction_point_1.X, rectangle.Center.Y + width_correction_point_1.Y + height_correction_point_1.Y); PointD correct_center_point_3 = new PointD(rectangle.Center.X - width_correction_point_1.X + height_correction_point_1.X, rectangle.Center.Y - width_correction_point_1.Y + height_correction_point_1.Y); /// 2st width_correction_angle_2 = Toolbox.ModuloPiAngleRadian(Angle - Math.PI / 2) + Math.PI; height_correction_angle_2 = Toolbox.ModuloPiAngleRadian(Angle) + Math.PI; //height_correction_angle_2 += (rectangle.Angle > 0) ? Math.PI : 0; PointD width_correction_point_2 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(width_correction_angle_2, width_correction_distance_2, 0)); PointD height_correction_point_2 = Toolbox.ConvertPolarToPointD(new PolarPointRssi(height_correction_angle_2, height_correction_distance_2, 0)); PointD correct_center_point_2 = new PointD(rectangle.Center.X + width_correction_point_2.X + height_correction_point_2.X, rectangle.Center.Y + width_correction_point_2.Y + height_correction_point_2.Y); //Console.ForegroundColor = ConsoleColor.Yellow; //Console.WriteLine("1 - " + (correct_center_point_1.X * 1000) + " : " + (correct_center_point_1.Y * 1000) + " - Angle: " + (rectangle.Angle * 180 / Math.PI)); //Console.ForegroundColor = ConsoleColor.Blue; //Console.WriteLine("2 - " + (correct_center_point_2.X * 1000) + " : " + (correct_center_point_2.Y * 1000) + " - Angle: " + (rectangle.Angle * 180 / Math.PI)); //Console.ResetColor(); //Console.WriteLine(""); RectangleOriented rectangle1 = new RectangleOriented(correct_center_point_1, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle); RectangleOriented rectangle2 = new RectangleOriented(correct_center_point_2, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle + Math.PI / 2); RectangleOriented rectangle3 = new RectangleOriented(correct_center_point_3, ConstVar.WIDTH_BOXSIZE, ConstVar.HEIGHT_BOXSIZE, Angle); return(new Tuple <RectangleOriented, RectangleOriented, RectangleOriented>(rectangle1, rectangle2, rectangle3)); }