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