init commit
This commit is contained in:
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class ClosePointsMergingOptimizer : IShapeOptimizer
|
||||
{
|
||||
private float maxDistanceToMerge = 10f;
|
||||
|
||||
public float MaxDistanceToMerge
|
||||
{
|
||||
get
|
||||
{
|
||||
return maxDistanceToMerge;
|
||||
}
|
||||
set
|
||||
{
|
||||
maxDistanceToMerge = System.Math.Max(0f, value);
|
||||
}
|
||||
}
|
||||
|
||||
public ClosePointsMergingOptimizer()
|
||||
{
|
||||
}
|
||||
|
||||
public ClosePointsMergingOptimizer(float maxDistanceToMerge)
|
||||
{
|
||||
this.maxDistanceToMerge = maxDistanceToMerge;
|
||||
}
|
||||
|
||||
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
|
||||
{
|
||||
List<IntPoint> list = new List<IntPoint>();
|
||||
if (shape.Count <= 3)
|
||||
{
|
||||
list.AddRange(shape);
|
||||
}
|
||||
else
|
||||
{
|
||||
float num = 0f;
|
||||
list.Add(shape[0]);
|
||||
int num2 = 1;
|
||||
int i = 1;
|
||||
for (int count = shape.Count; i < count; i++)
|
||||
{
|
||||
num = list[num2 - 1].DistanceTo(shape[i]);
|
||||
if (num <= maxDistanceToMerge && num2 + (count - i) > 3)
|
||||
{
|
||||
list[num2 - 1] = (list[num2 - 1] + shape[i]) / 2;
|
||||
continue;
|
||||
}
|
||||
list.Add(shape[i]);
|
||||
num2++;
|
||||
}
|
||||
if (num2 > 3)
|
||||
{
|
||||
num = list[num2 - 1].DistanceTo(list[0]);
|
||||
if (num <= maxDistanceToMerge)
|
||||
{
|
||||
list[0] = (list[num2 - 1] + list[0]) / 2;
|
||||
list.RemoveAt(num2 - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
return list;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,216 @@
|
||||
using System;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class CoplanarPosit
|
||||
{
|
||||
private const float ErrorLimit = 2f;
|
||||
|
||||
private float focalLength;
|
||||
|
||||
private Vector3[] modelPoints;
|
||||
|
||||
private Matrix3x3 modelVectors;
|
||||
|
||||
private Matrix3x3 modelPseudoInverse;
|
||||
|
||||
private Vector3 modelNormal;
|
||||
|
||||
private Matrix3x3 alternateRotation = default(Matrix3x3);
|
||||
|
||||
private Vector3 alternateTranslation = default(Vector3);
|
||||
|
||||
private float alternatePoseError;
|
||||
|
||||
private Matrix3x3 bestRotation = default(Matrix3x3);
|
||||
|
||||
private Vector3 bestTranslation = default(Vector3);
|
||||
|
||||
private float bestPoseError;
|
||||
|
||||
public Matrix3x3 BestEstimatedRotation => bestRotation;
|
||||
|
||||
public Vector3 BestEstimatedTranslation => bestTranslation;
|
||||
|
||||
public float BestEstimationError => bestPoseError;
|
||||
|
||||
public Matrix3x3 AlternateEstimatedRotation => alternateRotation;
|
||||
|
||||
public Vector3 AlternateEstimatedTranslation => alternateTranslation;
|
||||
|
||||
public float AlternateEstimationError => alternatePoseError;
|
||||
|
||||
public Vector3[] Model => (Vector3[])modelPoints.Clone();
|
||||
|
||||
public float FocalLength
|
||||
{
|
||||
get
|
||||
{
|
||||
return focalLength;
|
||||
}
|
||||
set
|
||||
{
|
||||
focalLength = value;
|
||||
}
|
||||
}
|
||||
|
||||
public CoplanarPosit(Vector3[] model, float focalLength)
|
||||
{
|
||||
if (model.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("The model must have 4 points.");
|
||||
}
|
||||
this.focalLength = focalLength;
|
||||
modelPoints = (Vector3[])model.Clone();
|
||||
modelVectors = Matrix3x3.CreateFromRows(model[1] - model[0], model[2] - model[0], model[3] - model[0]);
|
||||
modelVectors.SVD(out var u, out var e, out var v);
|
||||
modelPseudoInverse = v * Matrix3x3.CreateDiagonal(e.Inverse()) * u.Transpose();
|
||||
modelNormal = v.GetColumn(e.MinIndex);
|
||||
}
|
||||
|
||||
public void EstimatePose(Point[] points, out Matrix3x3 rotation, out Vector3 translation)
|
||||
{
|
||||
if (points.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("4 points must be be given for pose estimation.");
|
||||
}
|
||||
POS(points, new Vector3(1f), out var rotation2, out var rotation3, out var translation2, out var translation3);
|
||||
float num = Iterate(points, ref rotation2, ref translation2);
|
||||
float num2 = Iterate(points, ref rotation3, ref translation3);
|
||||
if (num < num2)
|
||||
{
|
||||
bestRotation = rotation2;
|
||||
bestTranslation = translation2;
|
||||
bestPoseError = num;
|
||||
alternateRotation = rotation3;
|
||||
alternateTranslation = translation3;
|
||||
alternatePoseError = num2;
|
||||
}
|
||||
else
|
||||
{
|
||||
bestRotation = rotation3;
|
||||
bestTranslation = translation3;
|
||||
bestPoseError = num2;
|
||||
alternateRotation = rotation2;
|
||||
alternateTranslation = translation2;
|
||||
alternatePoseError = num;
|
||||
}
|
||||
rotation = bestRotation;
|
||||
translation = bestTranslation;
|
||||
}
|
||||
|
||||
private float Iterate(Point[] points, ref Matrix3x3 rotation, ref Vector3 translation)
|
||||
{
|
||||
float num = float.MaxValue;
|
||||
float num2 = 0f;
|
||||
for (int i = 0; i < 100; i++)
|
||||
{
|
||||
Vector3 eps = modelVectors * rotation.GetRow(2) / translation.Z + 1f;
|
||||
POS(points, eps, out var rotation2, out var rotation3, out var translation2, out var translation3);
|
||||
float error = GetError(points, rotation2, translation2);
|
||||
float error2 = GetError(points, rotation3, translation3);
|
||||
if (error < error2)
|
||||
{
|
||||
rotation = rotation2;
|
||||
translation = translation2;
|
||||
num2 = error;
|
||||
}
|
||||
else
|
||||
{
|
||||
rotation = rotation3;
|
||||
translation = translation3;
|
||||
num2 = error2;
|
||||
}
|
||||
if (num2 <= 2f || num2 > num)
|
||||
{
|
||||
break;
|
||||
}
|
||||
num = num2;
|
||||
}
|
||||
return num2;
|
||||
}
|
||||
|
||||
private void POS(Point[] imagePoints, Vector3 eps, out Matrix3x3 rotation1, out Matrix3x3 rotation2, out Vector3 translation1, out Vector3 translation2)
|
||||
{
|
||||
Vector3 vector = new Vector3(imagePoints[1].X, imagePoints[2].X, imagePoints[3].X);
|
||||
Vector3 vector2 = new Vector3(imagePoints[1].Y, imagePoints[2].Y, imagePoints[3].Y);
|
||||
Vector3 vector3 = vector * eps - imagePoints[0].X;
|
||||
Vector3 vector4 = vector2 * eps - imagePoints[0].Y;
|
||||
Vector3 vector5 = modelPseudoInverse * vector3;
|
||||
Vector3 vector6 = modelPseudoInverse * vector4;
|
||||
Vector3 vector7 = default(Vector3);
|
||||
Vector3 vector8 = default(Vector3);
|
||||
Vector3 vector9 = default(Vector3);
|
||||
float num = vector6.Square - vector5.Square;
|
||||
float num2 = Vector3.Dot(vector5, vector6);
|
||||
float num3 = 0f;
|
||||
float num4 = 0f;
|
||||
if (num == 0f)
|
||||
{
|
||||
num4 = (float)(-System.Math.PI / 2.0 * (double)System.Math.Sign(num2));
|
||||
num3 = (float)System.Math.Sqrt(System.Math.Abs(2f * num2));
|
||||
}
|
||||
else
|
||||
{
|
||||
num3 = (float)System.Math.Sqrt(System.Math.Sqrt(num * num + 4f * num2 * num2));
|
||||
num4 = (float)System.Math.Atan(-2f * num2 / num);
|
||||
if (num < 0f)
|
||||
{
|
||||
num4 += (float)System.Math.PI;
|
||||
}
|
||||
num4 /= 2f;
|
||||
}
|
||||
float num5 = (float)((double)num3 * System.Math.Cos(num4));
|
||||
float num6 = (float)((double)num3 * System.Math.Sin(num4));
|
||||
vector7 = vector5 + modelNormal * num5;
|
||||
vector8 = vector6 + modelNormal * num6;
|
||||
float num7 = vector7.Normalize();
|
||||
float num8 = vector8.Normalize();
|
||||
vector9 = Vector3.Cross(vector7, vector8);
|
||||
rotation1 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
|
||||
float num9 = (num7 + num8) / 2f;
|
||||
Vector3 vector10 = rotation1 * modelPoints[0];
|
||||
translation1 = new Vector3(imagePoints[0].X / num9 - vector10.X, imagePoints[0].Y / num9 - vector10.Y, focalLength / num9);
|
||||
vector7 = vector5 - modelNormal * num5;
|
||||
vector8 = vector6 - modelNormal * num6;
|
||||
num7 = vector7.Normalize();
|
||||
num8 = vector8.Normalize();
|
||||
vector9 = Vector3.Cross(vector7, vector8);
|
||||
rotation2 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
|
||||
num9 = (num7 + num8) / 2f;
|
||||
vector10 = rotation2 * modelPoints[0];
|
||||
translation2 = new Vector3(imagePoints[0].X / num9 - vector10.X, imagePoints[0].Y / num9 - vector10.Y, focalLength / num9);
|
||||
}
|
||||
|
||||
private float GetError(Point[] imagePoints, Matrix3x3 rotation, Vector3 translation)
|
||||
{
|
||||
Vector3 vector = rotation * modelPoints[0] + translation;
|
||||
vector.X = vector.X * focalLength / vector.Z;
|
||||
vector.Y = vector.Y * focalLength / vector.Z;
|
||||
Vector3 vector2 = rotation * modelPoints[1] + translation;
|
||||
vector2.X = vector2.X * focalLength / vector2.Z;
|
||||
vector2.Y = vector2.Y * focalLength / vector2.Z;
|
||||
Vector3 vector3 = rotation * modelPoints[2] + translation;
|
||||
vector3.X = vector3.X * focalLength / vector3.Z;
|
||||
vector3.Y = vector3.Y * focalLength / vector3.Z;
|
||||
Vector3 vector4 = rotation * modelPoints[3] + translation;
|
||||
vector4.X = vector4.X * focalLength / vector4.Z;
|
||||
vector4.Y = vector4.Y * focalLength / vector4.Z;
|
||||
Point[] array = new Point[4]
|
||||
{
|
||||
new Point(vector.X, vector.Y),
|
||||
new Point(vector2.X, vector2.Y),
|
||||
new Point(vector3.X, vector3.Y),
|
||||
new Point(vector4.X, vector4.Y)
|
||||
};
|
||||
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(imagePoints[0], imagePoints[1], imagePoints[3]);
|
||||
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(imagePoints[1], imagePoints[2], imagePoints[0]);
|
||||
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(imagePoints[2], imagePoints[3], imagePoints[1]);
|
||||
float angleBetweenVectors4 = GeometryTools.GetAngleBetweenVectors(imagePoints[3], imagePoints[0], imagePoints[2]);
|
||||
float angleBetweenVectors5 = GeometryTools.GetAngleBetweenVectors(array[0], array[1], array[3]);
|
||||
float angleBetweenVectors6 = GeometryTools.GetAngleBetweenVectors(array[1], array[2], array[0]);
|
||||
float angleBetweenVectors7 = GeometryTools.GetAngleBetweenVectors(array[2], array[3], array[1]);
|
||||
float angleBetweenVectors8 = GeometryTools.GetAngleBetweenVectors(array[3], array[0], array[2]);
|
||||
return (System.Math.Abs(angleBetweenVectors - angleBetweenVectors5) + System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors6) + System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors7) + System.Math.Abs(angleBetweenVectors4 - angleBetweenVectors8)) / 4f;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class FlatAnglesOptimizer : IShapeOptimizer
|
||||
{
|
||||
private float maxAngleToKeep = 160f;
|
||||
|
||||
public float MaxAngleToKeep
|
||||
{
|
||||
get
|
||||
{
|
||||
return maxAngleToKeep;
|
||||
}
|
||||
set
|
||||
{
|
||||
maxAngleToKeep = System.Math.Min(180f, System.Math.Max(140f, value));
|
||||
}
|
||||
}
|
||||
|
||||
public FlatAnglesOptimizer()
|
||||
{
|
||||
}
|
||||
|
||||
public FlatAnglesOptimizer(float maxAngleToKeep)
|
||||
{
|
||||
this.maxAngleToKeep = maxAngleToKeep;
|
||||
}
|
||||
|
||||
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
|
||||
{
|
||||
List<IntPoint> list = new List<IntPoint>();
|
||||
if (shape.Count <= 3)
|
||||
{
|
||||
list.AddRange(shape);
|
||||
}
|
||||
else
|
||||
{
|
||||
float num = 0f;
|
||||
list.Add(shape[0]);
|
||||
list.Add(shape[1]);
|
||||
int num2 = 2;
|
||||
int i = 2;
|
||||
for (int count = shape.Count; i < count; i++)
|
||||
{
|
||||
list.Add(shape[i]);
|
||||
num2++;
|
||||
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 2], list[num2 - 3], list[num2 - 1]);
|
||||
if (num > maxAngleToKeep && (num2 > 3 || i < count - 1))
|
||||
{
|
||||
list.RemoveAt(num2 - 2);
|
||||
num2--;
|
||||
}
|
||||
}
|
||||
if (num2 > 3)
|
||||
{
|
||||
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 1], list[num2 - 2], list[0]);
|
||||
if (num > maxAngleToKeep)
|
||||
{
|
||||
list.RemoveAt(num2 - 1);
|
||||
num2--;
|
||||
}
|
||||
if (num2 > 3)
|
||||
{
|
||||
num = GeometryTools.GetAngleBetweenVectors(list[0], list[num2 - 1], list[1]);
|
||||
if (num > maxAngleToKeep)
|
||||
{
|
||||
list.RemoveAt(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return list;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
using System;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public static class GeometryTools
|
||||
{
|
||||
public static float GetAngleBetweenVectors(Point startPoint, Point vector1end, Point vector2end)
|
||||
{
|
||||
float num = vector1end.X - startPoint.X;
|
||||
float num2 = vector1end.Y - startPoint.Y;
|
||||
float num3 = vector2end.X - startPoint.X;
|
||||
float num4 = vector2end.Y - startPoint.Y;
|
||||
return (float)(System.Math.Acos((double)(num * num3 + num2 * num4) / (System.Math.Sqrt(num * num + num2 * num2) * System.Math.Sqrt(num3 * num3 + num4 * num4))) * 180.0 / System.Math.PI);
|
||||
}
|
||||
|
||||
public static float GetAngleBetweenLines(Point a1, Point a2, Point b1, Point b2)
|
||||
{
|
||||
Line line = Line.FromPoints(a1, a2);
|
||||
return line.GetAngleBetweenLines(Line.FromPoints(b1, b2));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,120 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class GrahamConvexHull : IConvexHullAlgorithm
|
||||
{
|
||||
private class PointToProcess : IComparable
|
||||
{
|
||||
public int X;
|
||||
|
||||
public int Y;
|
||||
|
||||
public float K;
|
||||
|
||||
public float Distance;
|
||||
|
||||
public PointToProcess(IntPoint point)
|
||||
{
|
||||
X = point.X;
|
||||
Y = point.Y;
|
||||
K = 0f;
|
||||
Distance = 0f;
|
||||
}
|
||||
|
||||
public int CompareTo(object obj)
|
||||
{
|
||||
PointToProcess pointToProcess = (PointToProcess)obj;
|
||||
if (!(K < pointToProcess.K))
|
||||
{
|
||||
if (!(K > pointToProcess.K))
|
||||
{
|
||||
if (!(Distance > pointToProcess.Distance))
|
||||
{
|
||||
if (!(Distance < pointToProcess.Distance))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
public IntPoint ToPoint()
|
||||
{
|
||||
return new IntPoint(X, Y);
|
||||
}
|
||||
}
|
||||
|
||||
public List<IntPoint> FindHull(List<IntPoint> points)
|
||||
{
|
||||
if (points.Count <= 3)
|
||||
{
|
||||
return new List<IntPoint>(points);
|
||||
}
|
||||
List<PointToProcess> list = new List<PointToProcess>();
|
||||
foreach (IntPoint point in points)
|
||||
{
|
||||
list.Add(new PointToProcess(point));
|
||||
}
|
||||
int index = 0;
|
||||
PointToProcess pointToProcess = list[0];
|
||||
int i = 1;
|
||||
for (int count = list.Count; i < count; i++)
|
||||
{
|
||||
if (list[i].X < pointToProcess.X || (list[i].X == pointToProcess.X && list[i].Y < pointToProcess.Y))
|
||||
{
|
||||
pointToProcess = list[i];
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
list.RemoveAt(index);
|
||||
int j = 0;
|
||||
for (int count2 = list.Count; j < count2; j++)
|
||||
{
|
||||
int num = list[j].X - pointToProcess.X;
|
||||
int num2 = list[j].Y - pointToProcess.Y;
|
||||
list[j].Distance = num * num + num2 * num2;
|
||||
list[j].K = ((num == 0) ? float.PositiveInfinity : ((float)num2 / (float)num));
|
||||
}
|
||||
list.Sort();
|
||||
List<PointToProcess> list2 = new List<PointToProcess>();
|
||||
list2.Add(pointToProcess);
|
||||
list2.Add(list[0]);
|
||||
list.RemoveAt(0);
|
||||
PointToProcess pointToProcess2 = list2[1];
|
||||
PointToProcess pointToProcess3 = list2[0];
|
||||
while (list.Count != 0)
|
||||
{
|
||||
PointToProcess pointToProcess4 = list[0];
|
||||
if (pointToProcess4.K == pointToProcess2.K || pointToProcess4.Distance == 0f)
|
||||
{
|
||||
list.RemoveAt(0);
|
||||
}
|
||||
else if ((pointToProcess4.X - pointToProcess3.X) * (pointToProcess2.Y - pointToProcess4.Y) - (pointToProcess2.X - pointToProcess4.X) * (pointToProcess4.Y - pointToProcess3.Y) < 0)
|
||||
{
|
||||
list2.Add(pointToProcess4);
|
||||
list.RemoveAt(0);
|
||||
pointToProcess3 = pointToProcess2;
|
||||
pointToProcess2 = pointToProcess4;
|
||||
}
|
||||
else
|
||||
{
|
||||
list2.RemoveAt(list2.Count - 1);
|
||||
pointToProcess2 = pointToProcess3;
|
||||
pointToProcess3 = list2[list2.Count - 2];
|
||||
}
|
||||
}
|
||||
List<IntPoint> list3 = new List<IntPoint>();
|
||||
foreach (PointToProcess item in list2)
|
||||
{
|
||||
list3.Add(item.ToPoint());
|
||||
}
|
||||
return list3;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public interface IConvexHullAlgorithm
|
||||
{
|
||||
List<IntPoint> FindHull(List<IntPoint> points);
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public interface IShapeOptimizer
|
||||
{
|
||||
List<IntPoint> OptimizeShape(List<IntPoint> shape);
|
||||
}
|
||||
206
output/Libraries/AForge.Math/AForge/Math/Geometry/Line.cs
Normal file
206
output/Libraries/AForge.Math/AForge/Math/Geometry/Line.cs
Normal file
@@ -0,0 +1,206 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public sealed class Line
|
||||
{
|
||||
private readonly float k;
|
||||
|
||||
private readonly float b;
|
||||
|
||||
public bool IsVertical => float.IsInfinity(k);
|
||||
|
||||
public bool IsHorizontal => k == 0f;
|
||||
|
||||
public float Slope => k;
|
||||
|
||||
public float Intercept => b;
|
||||
|
||||
public static Line FromPoints(Point point1, Point point2)
|
||||
{
|
||||
return new Line(point1, point2);
|
||||
}
|
||||
|
||||
public static Line FromSlopeIntercept(float slope, float intercept)
|
||||
{
|
||||
return new Line(slope, intercept);
|
||||
}
|
||||
|
||||
public static Line FromRTheta(float radius, float theta)
|
||||
{
|
||||
return new Line(radius, theta, unused: false);
|
||||
}
|
||||
|
||||
public static Line FromPointTheta(Point point, float theta)
|
||||
{
|
||||
return new Line(point, theta);
|
||||
}
|
||||
|
||||
private Line(Point start, Point end)
|
||||
{
|
||||
if (start == end)
|
||||
{
|
||||
throw new ArgumentException("Start point of the line cannot be the same as its end point.");
|
||||
}
|
||||
k = (end.Y - start.Y) / (end.X - start.X);
|
||||
b = (float.IsInfinity(k) ? start.X : (start.Y - k * start.X));
|
||||
}
|
||||
|
||||
private Line(float slope, float intercept)
|
||||
{
|
||||
k = slope;
|
||||
b = intercept;
|
||||
}
|
||||
|
||||
private Line(float radius, float theta, bool unused)
|
||||
{
|
||||
if (radius < 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException("radius", radius, "Must be non-negative");
|
||||
}
|
||||
theta *= (float)System.Math.PI / 180f;
|
||||
float num = (float)System.Math.Sin(theta);
|
||||
float num2 = (float)System.Math.Cos(theta);
|
||||
Point point = new Point(radius * num2, radius * num);
|
||||
k = (0f - num2) / num;
|
||||
if (!float.IsInfinity(k))
|
||||
{
|
||||
b = point.Y - k * point.X;
|
||||
}
|
||||
else
|
||||
{
|
||||
b = System.Math.Abs(radius);
|
||||
}
|
||||
}
|
||||
|
||||
private Line(Point point, float theta)
|
||||
{
|
||||
theta *= (float)System.Math.PI / 180f;
|
||||
k = (float)(-1.0 / System.Math.Tan(theta));
|
||||
if (!float.IsInfinity(k))
|
||||
{
|
||||
b = point.Y - k * point.X;
|
||||
}
|
||||
else
|
||||
{
|
||||
b = point.X;
|
||||
}
|
||||
}
|
||||
|
||||
public float GetAngleBetweenLines(Line secondLine)
|
||||
{
|
||||
float num = secondLine.k;
|
||||
bool isVertical = IsVertical;
|
||||
bool isVertical2 = secondLine.IsVertical;
|
||||
if (k == num || (isVertical && isVertical2))
|
||||
{
|
||||
return 0f;
|
||||
}
|
||||
float num2 = 0f;
|
||||
if (isVertical || isVertical2)
|
||||
{
|
||||
num2 = ((!isVertical) ? ((float)(System.Math.PI / 2.0 - System.Math.Atan(k) * (double)System.Math.Sign(k))) : ((float)(System.Math.PI / 2.0 - System.Math.Atan(num) * (double)System.Math.Sign(num))));
|
||||
}
|
||||
else
|
||||
{
|
||||
float num3 = ((num > k) ? (num - k) : (k - num)) / (1f + k * num);
|
||||
num2 = (float)System.Math.Atan(num3);
|
||||
}
|
||||
num2 *= 57.29578f;
|
||||
if (num2 < 0f)
|
||||
{
|
||||
num2 = 0f - num2;
|
||||
}
|
||||
return num2;
|
||||
}
|
||||
|
||||
public Point? GetIntersectionWith(Line secondLine)
|
||||
{
|
||||
float num = secondLine.k;
|
||||
float num2 = secondLine.b;
|
||||
bool isVertical = IsVertical;
|
||||
bool isVertical2 = secondLine.IsVertical;
|
||||
Point? result = null;
|
||||
if (k == num || (isVertical && isVertical2))
|
||||
{
|
||||
if (b == num2)
|
||||
{
|
||||
throw new InvalidOperationException("Identical lines do not have an intersection point.");
|
||||
}
|
||||
}
|
||||
else if (isVertical)
|
||||
{
|
||||
result = new Point(b, num * b + num2);
|
||||
}
|
||||
else if (isVertical2)
|
||||
{
|
||||
result = new Point(num2, k * num2 + b);
|
||||
}
|
||||
else
|
||||
{
|
||||
float num3 = (num2 - b) / (k - num);
|
||||
result = new Point(num3, k * num3 + b);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public Point? GetIntersectionWith(LineSegment other)
|
||||
{
|
||||
return other.GetIntersectionWith(this);
|
||||
}
|
||||
|
||||
public float DistanceToPoint(Point point)
|
||||
{
|
||||
if (!IsVertical)
|
||||
{
|
||||
float num = (float)System.Math.Sqrt(k * k + 1f);
|
||||
return System.Math.Abs((k * point.X + b - point.Y) / num);
|
||||
}
|
||||
return System.Math.Abs(b - point.X);
|
||||
}
|
||||
|
||||
public static bool operator ==(Line line1, Line line2)
|
||||
{
|
||||
if (object.ReferenceEquals(line1, line2))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
if ((object)line1 == null || (object)line2 == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (line1.k == line2.k)
|
||||
{
|
||||
return line1.b == line2.b;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool operator !=(Line line1, Line line2)
|
||||
{
|
||||
return !(line1 == line2);
|
||||
}
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (!(obj is Line))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return this == (Line)obj;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
float num = k;
|
||||
int hashCode = num.GetHashCode();
|
||||
float num2 = b;
|
||||
return hashCode + num2.GetHashCode();
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return string.Format(CultureInfo.InvariantCulture, "k = {0}, b = {1}", new object[2] { k, b });
|
||||
}
|
||||
}
|
||||
158
output/Libraries/AForge.Math/AForge/Math/Geometry/LineSegment.cs
Normal file
158
output/Libraries/AForge.Math/AForge/Math/Geometry/LineSegment.cs
Normal file
@@ -0,0 +1,158 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public sealed class LineSegment
|
||||
{
|
||||
private enum ProjectionLocation
|
||||
{
|
||||
RayA,
|
||||
SegmentAB,
|
||||
RayB
|
||||
}
|
||||
|
||||
private readonly Point start;
|
||||
|
||||
private readonly Point end;
|
||||
|
||||
private readonly Line line;
|
||||
|
||||
public Point Start => start;
|
||||
|
||||
public Point End => end;
|
||||
|
||||
public float Length => start.DistanceTo(end);
|
||||
|
||||
public LineSegment(Point start, Point end)
|
||||
{
|
||||
line = Line.FromPoints(start, end);
|
||||
this.start = start;
|
||||
this.end = end;
|
||||
}
|
||||
|
||||
public static explicit operator Line(LineSegment segment)
|
||||
{
|
||||
return segment.line;
|
||||
}
|
||||
|
||||
public float DistanceToPoint(Point point)
|
||||
{
|
||||
return LocateProjection(point) switch
|
||||
{
|
||||
ProjectionLocation.RayA => point.DistanceTo(start),
|
||||
ProjectionLocation.RayB => point.DistanceTo(end),
|
||||
_ => line.DistanceToPoint(point),
|
||||
};
|
||||
}
|
||||
|
||||
public Point? GetIntersectionWith(LineSegment other)
|
||||
{
|
||||
Point? result = null;
|
||||
if (line.Slope == other.line.Slope || (line.IsVertical && other.line.IsVertical))
|
||||
{
|
||||
if (line.Intercept == other.line.Intercept)
|
||||
{
|
||||
ProjectionLocation projectionLocation = LocateProjection(other.start);
|
||||
ProjectionLocation projectionLocation2 = LocateProjection(other.end);
|
||||
if (projectionLocation != ProjectionLocation.SegmentAB && projectionLocation == projectionLocation2)
|
||||
{
|
||||
result = null;
|
||||
}
|
||||
else if ((start == other.start && projectionLocation2 == ProjectionLocation.RayA) || (start == other.end && projectionLocation == ProjectionLocation.RayA))
|
||||
{
|
||||
result = start;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((!(end == other.start) || projectionLocation2 != ProjectionLocation.RayB) && (!(end == other.end) || projectionLocation != ProjectionLocation.RayB))
|
||||
{
|
||||
throw new InvalidOperationException("Overlapping segments do not have a single intersection point.");
|
||||
}
|
||||
result = end;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
result = GetIntersectionWith(other.line);
|
||||
if (result.HasValue && other.LocateProjection(result.Value) != ProjectionLocation.SegmentAB)
|
||||
{
|
||||
result = null;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public Point? GetIntersectionWith(Line other)
|
||||
{
|
||||
Point? result;
|
||||
if (line.Slope == other.Slope || (line.IsVertical && other.IsVertical))
|
||||
{
|
||||
if (line.Intercept == other.Intercept)
|
||||
{
|
||||
throw new InvalidOperationException("Segment is a portion of the specified line.");
|
||||
}
|
||||
result = null;
|
||||
}
|
||||
else
|
||||
{
|
||||
result = line.GetIntersectionWith(other);
|
||||
}
|
||||
if (result.HasValue && LocateProjection(result.Value) != ProjectionLocation.SegmentAB)
|
||||
{
|
||||
result = null;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
private ProjectionLocation LocateProjection(Point point)
|
||||
{
|
||||
Point point2 = end - start;
|
||||
Point point3 = point - start;
|
||||
float num = point3.X * point2.X + point3.Y * point2.Y;
|
||||
float num2 = point2.X * point2.X + point2.Y * point2.Y;
|
||||
return (!(num < 0f)) ? ((!(num > num2)) ? ProjectionLocation.SegmentAB : ProjectionLocation.RayB) : ProjectionLocation.RayA;
|
||||
}
|
||||
|
||||
public static bool operator ==(LineSegment line1, LineSegment line2)
|
||||
{
|
||||
if (object.ReferenceEquals(line1, line2))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
if ((object)line1 == null || (object)line2 == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (line1.start == line2.start)
|
||||
{
|
||||
return line1.end == line2.end;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool operator !=(LineSegment line1, LineSegment line2)
|
||||
{
|
||||
return !(line1 == line2);
|
||||
}
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (!(obj is LineSegment))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return this == (LineSegment)obj;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return start.GetHashCode() + end.GetHashCode();
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return string.Format(CultureInfo.InvariantCulture, "({0}) -> ({1})", new object[2] { start, end });
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class LineStraighteningOptimizer : IShapeOptimizer
|
||||
{
|
||||
private float maxDistanceToRemove = 5f;
|
||||
|
||||
public float MaxDistanceToRemove
|
||||
{
|
||||
get
|
||||
{
|
||||
return maxDistanceToRemove;
|
||||
}
|
||||
set
|
||||
{
|
||||
maxDistanceToRemove = System.Math.Max(0f, value);
|
||||
}
|
||||
}
|
||||
|
||||
public LineStraighteningOptimizer()
|
||||
{
|
||||
}
|
||||
|
||||
public LineStraighteningOptimizer(float maxDistanceToRemove)
|
||||
{
|
||||
this.maxDistanceToRemove = maxDistanceToRemove;
|
||||
}
|
||||
|
||||
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
|
||||
{
|
||||
List<IntPoint> list = new List<IntPoint>();
|
||||
List<IntPoint> list2 = new List<IntPoint>();
|
||||
if (shape.Count <= 3)
|
||||
{
|
||||
list.AddRange(shape);
|
||||
}
|
||||
else
|
||||
{
|
||||
float distance = 0f;
|
||||
list.Add(shape[0]);
|
||||
list.Add(shape[1]);
|
||||
int num = 2;
|
||||
int i = 2;
|
||||
for (int count = shape.Count; i < count; i++)
|
||||
{
|
||||
list.Add(shape[i]);
|
||||
num++;
|
||||
list2.Add(list[num - 2]);
|
||||
PointsCloud.GetFurthestPointFromLine(list2, list[num - 3], list[num - 1], out distance);
|
||||
if (distance <= maxDistanceToRemove && (num > 3 || i < count - 1))
|
||||
{
|
||||
list.RemoveAt(num - 2);
|
||||
num--;
|
||||
}
|
||||
else
|
||||
{
|
||||
list2.Clear();
|
||||
}
|
||||
}
|
||||
if (num > 3)
|
||||
{
|
||||
list2.Add(list[num - 1]);
|
||||
PointsCloud.GetFurthestPointFromLine(list2, list[num - 2], list[0], out distance);
|
||||
if (distance <= maxDistanceToRemove)
|
||||
{
|
||||
list.RemoveAt(num - 1);
|
||||
num--;
|
||||
}
|
||||
else
|
||||
{
|
||||
list2.Clear();
|
||||
}
|
||||
if (num > 3)
|
||||
{
|
||||
list2.Add(list[0]);
|
||||
PointsCloud.GetFurthestPointFromLine(list2, list[num - 1], list[1], out distance);
|
||||
if (distance <= maxDistanceToRemove)
|
||||
{
|
||||
list.RemoveAt(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return list;
|
||||
}
|
||||
}
|
||||
321
output/Libraries/AForge.Math/AForge/Math/Geometry/PointsCloud.cs
Normal file
321
output/Libraries/AForge.Math/AForge/Math/Geometry/PointsCloud.cs
Normal file
@@ -0,0 +1,321 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public static class PointsCloud
|
||||
{
|
||||
private static float quadrilateralRelativeDistortionLimit = 0.1f;
|
||||
|
||||
public static float QuadrilateralRelativeDistortionLimit
|
||||
{
|
||||
get
|
||||
{
|
||||
return quadrilateralRelativeDistortionLimit;
|
||||
}
|
||||
set
|
||||
{
|
||||
quadrilateralRelativeDistortionLimit = System.Math.Max(0f, System.Math.Min(0.25f, value));
|
||||
}
|
||||
}
|
||||
|
||||
public static void Shift(IList<IntPoint> cloud, IntPoint shift)
|
||||
{
|
||||
int i = 0;
|
||||
for (int count = cloud.Count; i < count; i++)
|
||||
{
|
||||
cloud[i] += shift;
|
||||
}
|
||||
}
|
||||
|
||||
public static void GetBoundingRectangle(IEnumerable<IntPoint> cloud, out IntPoint minXY, out IntPoint maxXY)
|
||||
{
|
||||
int num = int.MaxValue;
|
||||
int num2 = int.MinValue;
|
||||
int num3 = int.MaxValue;
|
||||
int num4 = int.MinValue;
|
||||
foreach (IntPoint item in cloud)
|
||||
{
|
||||
int x = item.X;
|
||||
int y = item.Y;
|
||||
if (x < num)
|
||||
{
|
||||
num = x;
|
||||
}
|
||||
if (x > num2)
|
||||
{
|
||||
num2 = x;
|
||||
}
|
||||
if (y < num3)
|
||||
{
|
||||
num3 = y;
|
||||
}
|
||||
if (y > num4)
|
||||
{
|
||||
num4 = y;
|
||||
}
|
||||
}
|
||||
if (num > num2)
|
||||
{
|
||||
throw new ArgumentException("List of points can not be empty.");
|
||||
}
|
||||
minXY = new IntPoint(num, num3);
|
||||
maxXY = new IntPoint(num2, num4);
|
||||
}
|
||||
|
||||
public static Point GetCenterOfGravity(IEnumerable<IntPoint> cloud)
|
||||
{
|
||||
int num = 0;
|
||||
float num2 = 0f;
|
||||
float num3 = 0f;
|
||||
foreach (IntPoint item in cloud)
|
||||
{
|
||||
num2 += (float)item.X;
|
||||
num3 += (float)item.Y;
|
||||
num++;
|
||||
}
|
||||
num2 /= (float)num;
|
||||
num3 /= (float)num;
|
||||
return new Point(num2, num3);
|
||||
}
|
||||
|
||||
public static IntPoint GetFurthestPoint(IEnumerable<IntPoint> cloud, IntPoint referencePoint)
|
||||
{
|
||||
IntPoint result = referencePoint;
|
||||
float num = -1f;
|
||||
int x = referencePoint.X;
|
||||
int y = referencePoint.Y;
|
||||
foreach (IntPoint item in cloud)
|
||||
{
|
||||
int num2 = x - item.X;
|
||||
int num3 = y - item.Y;
|
||||
float num4 = num2 * num2 + num3 * num3;
|
||||
if (num4 > num)
|
||||
{
|
||||
num = num4;
|
||||
result = item;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public static void GetFurthestPointsFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out IntPoint furthestPoint1, out IntPoint furthestPoint2)
|
||||
{
|
||||
GetFurthestPointsFromLine(cloud, linePoint1, linePoint2, out furthestPoint1, out var _, out furthestPoint2, out var _);
|
||||
}
|
||||
|
||||
public static void GetFurthestPointsFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out IntPoint furthestPoint1, out float distance1, out IntPoint furthestPoint2, out float distance2)
|
||||
{
|
||||
furthestPoint1 = linePoint1;
|
||||
distance1 = 0f;
|
||||
furthestPoint2 = linePoint2;
|
||||
distance2 = 0f;
|
||||
if (linePoint2.X != linePoint1.X)
|
||||
{
|
||||
float num = (float)(linePoint2.Y - linePoint1.Y) / (float)(linePoint2.X - linePoint1.X);
|
||||
float num2 = (float)linePoint1.Y - num * (float)linePoint1.X;
|
||||
float num3 = (float)System.Math.Sqrt(num * num + 1f);
|
||||
float num4 = 0f;
|
||||
foreach (IntPoint item in cloud)
|
||||
{
|
||||
num4 = (num * (float)item.X + num2 - (float)item.Y) / num3;
|
||||
if (num4 > distance1)
|
||||
{
|
||||
distance1 = num4;
|
||||
furthestPoint1 = item;
|
||||
}
|
||||
if (num4 < distance2)
|
||||
{
|
||||
distance2 = num4;
|
||||
furthestPoint2 = item;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int x = linePoint1.X;
|
||||
float num5 = 0f;
|
||||
foreach (IntPoint item2 in cloud)
|
||||
{
|
||||
num5 = x - item2.X;
|
||||
if (num5 > distance1)
|
||||
{
|
||||
distance1 = num5;
|
||||
furthestPoint1 = item2;
|
||||
}
|
||||
if (num5 < distance2)
|
||||
{
|
||||
distance2 = num5;
|
||||
furthestPoint2 = item2;
|
||||
}
|
||||
}
|
||||
}
|
||||
distance2 = 0f - distance2;
|
||||
}
|
||||
|
||||
public static IntPoint GetFurthestPointFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2)
|
||||
{
|
||||
float distance;
|
||||
return GetFurthestPointFromLine(cloud, linePoint1, linePoint2, out distance);
|
||||
}
|
||||
|
||||
public static IntPoint GetFurthestPointFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out float distance)
|
||||
{
|
||||
IntPoint result = linePoint1;
|
||||
distance = 0f;
|
||||
if (linePoint2.X != linePoint1.X)
|
||||
{
|
||||
float num = (float)(linePoint2.Y - linePoint1.Y) / (float)(linePoint2.X - linePoint1.X);
|
||||
float num2 = (float)linePoint1.Y - num * (float)linePoint1.X;
|
||||
float num3 = (float)System.Math.Sqrt(num * num + 1f);
|
||||
float num4 = 0f;
|
||||
foreach (IntPoint item in cloud)
|
||||
{
|
||||
num4 = System.Math.Abs((num * (float)item.X + num2 - (float)item.Y) / num3);
|
||||
if (num4 > distance)
|
||||
{
|
||||
distance = num4;
|
||||
result = item;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int x = linePoint1.X;
|
||||
float num5 = 0f;
|
||||
foreach (IntPoint item2 in cloud)
|
||||
{
|
||||
distance = System.Math.Abs(x - item2.X);
|
||||
if (num5 > distance)
|
||||
{
|
||||
distance = num5;
|
||||
result = item2;
|
||||
}
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<IntPoint> FindQuadrilateralCorners(IEnumerable<IntPoint> cloud)
|
||||
{
|
||||
List<IntPoint> list = new List<IntPoint>();
|
||||
GetBoundingRectangle(cloud, out var minXY, out var maxXY);
|
||||
IntPoint intPoint = maxXY - minXY;
|
||||
IntPoint referencePoint = minXY + intPoint / 2;
|
||||
float num = quadrilateralRelativeDistortionLimit * (float)(intPoint.X + intPoint.Y) / 2f;
|
||||
IntPoint furthestPoint = GetFurthestPoint(cloud, referencePoint);
|
||||
IntPoint furthestPoint2 = GetFurthestPoint(cloud, furthestPoint);
|
||||
list.Add(furthestPoint);
|
||||
list.Add(furthestPoint2);
|
||||
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint2, out var furthestPoint3, out var distance, out var furthestPoint4, out var distance2);
|
||||
if ((distance >= num && distance2 >= num) || (distance < num && distance != 0f && distance2 < num && distance2 != 0f))
|
||||
{
|
||||
if (!list.Contains(furthestPoint3))
|
||||
{
|
||||
list.Add(furthestPoint3);
|
||||
}
|
||||
if (!list.Contains(furthestPoint4))
|
||||
{
|
||||
list.Add(furthestPoint4);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
IntPoint furthestPoint5 = ((distance > distance2) ? furthestPoint3 : furthestPoint4);
|
||||
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint5, out furthestPoint3, out distance, out furthestPoint4, out distance2);
|
||||
bool flag = false;
|
||||
if (distance >= num && distance2 >= num)
|
||||
{
|
||||
if (furthestPoint4.DistanceTo(furthestPoint2) > furthestPoint3.DistanceTo(furthestPoint2))
|
||||
{
|
||||
furthestPoint3 = furthestPoint4;
|
||||
}
|
||||
flag = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
GetFurthestPointsFromLine(cloud, furthestPoint2, furthestPoint5, out furthestPoint3, out distance, out furthestPoint4, out distance2);
|
||||
if (distance >= num && distance2 >= num)
|
||||
{
|
||||
if (furthestPoint4.DistanceTo(furthestPoint) > furthestPoint3.DistanceTo(furthestPoint))
|
||||
{
|
||||
furthestPoint3 = furthestPoint4;
|
||||
}
|
||||
flag = true;
|
||||
}
|
||||
}
|
||||
if (!flag)
|
||||
{
|
||||
list.Add(furthestPoint5);
|
||||
}
|
||||
else
|
||||
{
|
||||
list.Add(furthestPoint3);
|
||||
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint3, out furthestPoint5, out var distance3, out furthestPoint4, out distance2);
|
||||
if (distance2 >= num && distance3 >= num)
|
||||
{
|
||||
if (furthestPoint5.DistanceTo(furthestPoint2) > furthestPoint4.DistanceTo(furthestPoint2))
|
||||
{
|
||||
furthestPoint4 = furthestPoint5;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
GetFurthestPointsFromLine(cloud, furthestPoint2, furthestPoint3, out furthestPoint5, out distance3, out furthestPoint4, out distance2);
|
||||
if (furthestPoint5.DistanceTo(furthestPoint) > furthestPoint4.DistanceTo(furthestPoint) && furthestPoint5 != furthestPoint2 && furthestPoint5 != furthestPoint3)
|
||||
{
|
||||
furthestPoint4 = furthestPoint5;
|
||||
}
|
||||
}
|
||||
if (furthestPoint4 != furthestPoint && furthestPoint4 != furthestPoint2 && furthestPoint4 != furthestPoint3)
|
||||
{
|
||||
list.Add(furthestPoint4);
|
||||
}
|
||||
}
|
||||
}
|
||||
int i = 1;
|
||||
for (int count = list.Count; i < count; i++)
|
||||
{
|
||||
if (list[i].X < list[0].X || (list[i].X == list[0].X && list[i].Y < list[0].Y))
|
||||
{
|
||||
IntPoint value = list[i];
|
||||
list[i] = list[0];
|
||||
list[0] = value;
|
||||
}
|
||||
}
|
||||
float num2 = ((list[1].X != list[0].X) ? ((float)(list[1].Y - list[0].Y) / (float)(list[1].X - list[0].X)) : ((list[1].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
|
||||
float num3 = ((list[2].X != list[0].X) ? ((float)(list[2].Y - list[0].Y) / (float)(list[2].X - list[0].X)) : ((list[2].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
|
||||
if (num3 < num2)
|
||||
{
|
||||
IntPoint value2 = list[1];
|
||||
list[1] = list[2];
|
||||
list[2] = value2;
|
||||
float num4 = num2;
|
||||
num2 = num3;
|
||||
num3 = num4;
|
||||
}
|
||||
if (list.Count == 4)
|
||||
{
|
||||
float num5 = ((list[3].X != list[0].X) ? ((float)(list[3].Y - list[0].Y) / (float)(list[3].X - list[0].X)) : ((list[3].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
|
||||
if (num5 < num2)
|
||||
{
|
||||
IntPoint value3 = list[1];
|
||||
list[1] = list[3];
|
||||
list[3] = value3;
|
||||
float num6 = num2;
|
||||
num2 = num5;
|
||||
num5 = num6;
|
||||
}
|
||||
if (num5 < num3)
|
||||
{
|
||||
IntPoint value4 = list[2];
|
||||
list[2] = list[3];
|
||||
list[3] = value4;
|
||||
float num7 = num3;
|
||||
num3 = num5;
|
||||
num5 = num7;
|
||||
}
|
||||
}
|
||||
return list;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public enum PolygonSubType
|
||||
{
|
||||
Unknown,
|
||||
Trapezoid,
|
||||
Parallelogram,
|
||||
Rectangle,
|
||||
Rhombus,
|
||||
Square,
|
||||
EquilateralTriangle,
|
||||
IsoscelesTriangle,
|
||||
RectangledTriangle,
|
||||
RectangledIsoscelesTriangle
|
||||
}
|
||||
84
output/Libraries/AForge.Math/AForge/Math/Geometry/Posit.cs
Normal file
84
output/Libraries/AForge.Math/AForge/Math/Geometry/Posit.cs
Normal file
@@ -0,0 +1,84 @@
|
||||
using System;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class Posit
|
||||
{
|
||||
private const float stop_epsilon = 0.0001f;
|
||||
|
||||
private float focalLength;
|
||||
|
||||
private Vector3[] modelPoints;
|
||||
|
||||
private Matrix3x3 modelVectors;
|
||||
|
||||
private Matrix3x3 modelPseudoInverse;
|
||||
|
||||
public Vector3[] Model => (Vector3[])modelPoints.Clone();
|
||||
|
||||
public float FocalLength
|
||||
{
|
||||
get
|
||||
{
|
||||
return focalLength;
|
||||
}
|
||||
set
|
||||
{
|
||||
focalLength = value;
|
||||
}
|
||||
}
|
||||
|
||||
public Posit(Vector3[] model, float focalLength)
|
||||
{
|
||||
if (model.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("The model must have 4 points.");
|
||||
}
|
||||
this.focalLength = focalLength;
|
||||
modelPoints = (Vector3[])model.Clone();
|
||||
modelVectors = Matrix3x3.CreateFromRows(model[1] - model[0], model[2] - model[0], model[3] - model[0]);
|
||||
modelPseudoInverse = modelVectors.PseudoInverse();
|
||||
}
|
||||
|
||||
public void EstimatePose(Point[] points, out Matrix3x3 rotation, out Vector3 translation)
|
||||
{
|
||||
if (points.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("4 points must be be given for pose estimation.");
|
||||
}
|
||||
float num = 0f;
|
||||
float num2 = 1f;
|
||||
Vector3 vector = new Vector3(points[0].X);
|
||||
Vector3 vector2 = new Vector3(points[0].Y);
|
||||
Vector3 vector3 = new Vector3(points[1].X, points[2].X, points[3].X);
|
||||
Vector3 vector4 = new Vector3(points[1].Y, points[2].Y, points[3].Y);
|
||||
int i = 0;
|
||||
Vector3 vector5 = default(Vector3);
|
||||
Vector3 vector6 = default(Vector3);
|
||||
Vector3 vector7 = default(Vector3);
|
||||
Vector3 vector8 = default(Vector3);
|
||||
Vector3 vector9 = default(Vector3);
|
||||
Vector3 vector10 = new Vector3(1f);
|
||||
for (; i < 100; i++)
|
||||
{
|
||||
vector8 = vector3 * vector10 - vector;
|
||||
vector9 = vector4 * vector10 - vector2;
|
||||
vector5 = modelPseudoInverse * vector8;
|
||||
vector6 = modelPseudoInverse * vector9;
|
||||
float num3 = vector5.Normalize();
|
||||
float num4 = vector6.Normalize();
|
||||
num2 = (num3 + num4) / 2f;
|
||||
vector7 = Vector3.Cross(vector5, vector6);
|
||||
num = focalLength / num2;
|
||||
Vector3 vector11 = vector10;
|
||||
vector10 = modelVectors * vector7 / num + 1f;
|
||||
if ((vector10 - vector11).Abs().Max < 0.0001f)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
rotation = Matrix3x3.CreateFromRows(vector5, vector6, vector7);
|
||||
Vector3 vector12 = rotation * modelPoints[0];
|
||||
translation = new Vector3(points[0].X / num2 - vector12.X, points[0].Y / num2 - vector12.Y, focalLength / num2);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public enum ShapeType
|
||||
{
|
||||
Unknown,
|
||||
Circle,
|
||||
Triangle,
|
||||
Quadrilateral
|
||||
}
|
||||
@@ -0,0 +1,253 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AForge.Math.Geometry;
|
||||
|
||||
public class SimpleShapeChecker
|
||||
{
|
||||
private FlatAnglesOptimizer shapeOptimizer = new FlatAnglesOptimizer(160f);
|
||||
|
||||
private float minAcceptableDistortion = 0.5f;
|
||||
|
||||
private float relativeDistortionLimit = 0.03f;
|
||||
|
||||
private float angleError = 7f;
|
||||
|
||||
private float lengthError = 0.1f;
|
||||
|
||||
public float MinAcceptableDistortion
|
||||
{
|
||||
get
|
||||
{
|
||||
return minAcceptableDistortion;
|
||||
}
|
||||
set
|
||||
{
|
||||
minAcceptableDistortion = System.Math.Max(0f, value);
|
||||
}
|
||||
}
|
||||
|
||||
public float RelativeDistortionLimit
|
||||
{
|
||||
get
|
||||
{
|
||||
return relativeDistortionLimit;
|
||||
}
|
||||
set
|
||||
{
|
||||
relativeDistortionLimit = System.Math.Max(0f, System.Math.Min(1f, value));
|
||||
}
|
||||
}
|
||||
|
||||
public float AngleError
|
||||
{
|
||||
get
|
||||
{
|
||||
return angleError;
|
||||
}
|
||||
set
|
||||
{
|
||||
angleError = System.Math.Max(0f, System.Math.Min(20f, value));
|
||||
}
|
||||
}
|
||||
|
||||
public float LengthError
|
||||
{
|
||||
get
|
||||
{
|
||||
return lengthError;
|
||||
}
|
||||
set
|
||||
{
|
||||
lengthError = System.Math.Max(0f, System.Math.Min(1f, value));
|
||||
}
|
||||
}
|
||||
|
||||
public ShapeType CheckShapeType(List<IntPoint> edgePoints)
|
||||
{
|
||||
if (IsCircle(edgePoints))
|
||||
{
|
||||
return ShapeType.Circle;
|
||||
}
|
||||
if (IsConvexPolygon(edgePoints, out var corners))
|
||||
{
|
||||
if (corners.Count != 4)
|
||||
{
|
||||
return ShapeType.Triangle;
|
||||
}
|
||||
return ShapeType.Quadrilateral;
|
||||
}
|
||||
return ShapeType.Unknown;
|
||||
}
|
||||
|
||||
public bool IsCircle(List<IntPoint> edgePoints)
|
||||
{
|
||||
Point center;
|
||||
float radius;
|
||||
return IsCircle(edgePoints, out center, out radius);
|
||||
}
|
||||
|
||||
public bool IsCircle(List<IntPoint> edgePoints, out Point center, out float radius)
|
||||
{
|
||||
if (edgePoints.Count < 8)
|
||||
{
|
||||
center = new Point(0f, 0f);
|
||||
radius = 0f;
|
||||
return false;
|
||||
}
|
||||
PointsCloud.GetBoundingRectangle(edgePoints, out var minXY, out var maxXY);
|
||||
IntPoint intPoint = maxXY - minXY;
|
||||
center = minXY + (Point)intPoint / 2f;
|
||||
radius = ((float)intPoint.X + (float)intPoint.Y) / 4f;
|
||||
float num = 0f;
|
||||
int i = 0;
|
||||
for (int count = edgePoints.Count; i < count; i++)
|
||||
{
|
||||
num += System.Math.Abs(center.DistanceTo(edgePoints[i]) - radius);
|
||||
}
|
||||
num /= (float)edgePoints.Count;
|
||||
float num2 = System.Math.Max(minAcceptableDistortion, ((float)intPoint.X + (float)intPoint.Y) / 2f * relativeDistortionLimit);
|
||||
return num <= num2;
|
||||
}
|
||||
|
||||
public bool IsQuadrilateral(List<IntPoint> edgePoints)
|
||||
{
|
||||
List<IntPoint> corners;
|
||||
return IsQuadrilateral(edgePoints, out corners);
|
||||
}
|
||||
|
||||
public bool IsQuadrilateral(List<IntPoint> edgePoints, out List<IntPoint> corners)
|
||||
{
|
||||
corners = GetShapeCorners(edgePoints);
|
||||
if (corners.Count != 4)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return CheckIfPointsFitShape(edgePoints, corners);
|
||||
}
|
||||
|
||||
public bool IsTriangle(List<IntPoint> edgePoints)
|
||||
{
|
||||
List<IntPoint> corners;
|
||||
return IsTriangle(edgePoints, out corners);
|
||||
}
|
||||
|
||||
public bool IsTriangle(List<IntPoint> edgePoints, out List<IntPoint> corners)
|
||||
{
|
||||
corners = GetShapeCorners(edgePoints);
|
||||
if (corners.Count != 3)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return CheckIfPointsFitShape(edgePoints, corners);
|
||||
}
|
||||
|
||||
public bool IsConvexPolygon(List<IntPoint> edgePoints, out List<IntPoint> corners)
|
||||
{
|
||||
corners = GetShapeCorners(edgePoints);
|
||||
return CheckIfPointsFitShape(edgePoints, corners);
|
||||
}
|
||||
|
||||
public PolygonSubType CheckPolygonSubType(List<IntPoint> corners)
|
||||
{
|
||||
PolygonSubType polygonSubType = PolygonSubType.Unknown;
|
||||
PointsCloud.GetBoundingRectangle(corners, out var minXY, out var maxXY);
|
||||
IntPoint intPoint = maxXY - minXY;
|
||||
float num = lengthError * (float)(intPoint.X + intPoint.Y) / 2f;
|
||||
if (corners.Count == 3)
|
||||
{
|
||||
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(corners[0], corners[1], corners[2]);
|
||||
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(corners[1], corners[2], corners[0]);
|
||||
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(corners[2], corners[0], corners[1]);
|
||||
if (System.Math.Abs(angleBetweenVectors - 60f) <= angleError && System.Math.Abs(angleBetweenVectors2 - 60f) <= angleError && System.Math.Abs(angleBetweenVectors3 - 60f) <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.EquilateralTriangle;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (System.Math.Abs(angleBetweenVectors - angleBetweenVectors2) <= angleError || System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors3) <= angleError || System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors) <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.IsoscelesTriangle;
|
||||
}
|
||||
if (System.Math.Abs(angleBetweenVectors - 90f) <= angleError || System.Math.Abs(angleBetweenVectors2 - 90f) <= angleError || System.Math.Abs(angleBetweenVectors3 - 90f) <= angleError)
|
||||
{
|
||||
polygonSubType = ((polygonSubType == PolygonSubType.IsoscelesTriangle) ? PolygonSubType.RectangledIsoscelesTriangle : PolygonSubType.RectangledTriangle);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (corners.Count == 4)
|
||||
{
|
||||
float angleBetweenLines = GeometryTools.GetAngleBetweenLines(corners[0], corners[1], corners[2], corners[3]);
|
||||
float angleBetweenLines2 = GeometryTools.GetAngleBetweenLines(corners[1], corners[2], corners[3], corners[0]);
|
||||
if (angleBetweenLines <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.Trapezoid;
|
||||
if (angleBetweenLines2 <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.Parallelogram;
|
||||
if (System.Math.Abs(GeometryTools.GetAngleBetweenVectors(corners[1], corners[0], corners[2]) - 90f) <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.Rectangle;
|
||||
}
|
||||
float num2 = corners[0].DistanceTo(corners[1]);
|
||||
float num3 = corners[0].DistanceTo(corners[3]);
|
||||
if (System.Math.Abs(num2 - num3) <= num)
|
||||
{
|
||||
polygonSubType = ((polygonSubType == PolygonSubType.Parallelogram) ? PolygonSubType.Rhombus : PolygonSubType.Square);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (angleBetweenLines2 <= angleError)
|
||||
{
|
||||
polygonSubType = PolygonSubType.Trapezoid;
|
||||
}
|
||||
}
|
||||
return polygonSubType;
|
||||
}
|
||||
|
||||
public bool CheckIfPointsFitShape(List<IntPoint> edgePoints, List<IntPoint> corners)
|
||||
{
|
||||
int count = corners.Count;
|
||||
float[] array = new float[count];
|
||||
float[] array2 = new float[count];
|
||||
float[] array3 = new float[count];
|
||||
bool[] array4 = new bool[count];
|
||||
for (int i = 0; i < count; i++)
|
||||
{
|
||||
IntPoint intPoint = corners[i];
|
||||
IntPoint intPoint2 = ((i + 1 == count) ? corners[0] : corners[i + 1]);
|
||||
if (!(array4[i] = intPoint2.X == intPoint.X))
|
||||
{
|
||||
array[i] = (float)(intPoint2.Y - intPoint.Y) / (float)(intPoint2.X - intPoint.X);
|
||||
array2[i] = (float)intPoint.Y - array[i] * (float)intPoint.X;
|
||||
array3[i] = (float)System.Math.Sqrt(array[i] * array[i] + 1f);
|
||||
}
|
||||
}
|
||||
float num = 0f;
|
||||
int j = 0;
|
||||
for (int count2 = edgePoints.Count; j < count2; j++)
|
||||
{
|
||||
float num2 = float.MaxValue;
|
||||
for (int k = 0; k < count; k++)
|
||||
{
|
||||
float num3 = 0f;
|
||||
num3 = (array4[k] ? ((float)System.Math.Abs(edgePoints[j].X - corners[k].X)) : System.Math.Abs((array[k] * (float)edgePoints[j].X + array2[k] - (float)edgePoints[j].Y) / array3[k]));
|
||||
if (num3 < num2)
|
||||
{
|
||||
num2 = num3;
|
||||
}
|
||||
}
|
||||
num += num2;
|
||||
}
|
||||
num /= (float)edgePoints.Count;
|
||||
PointsCloud.GetBoundingRectangle(corners, out var minXY, out var maxXY);
|
||||
IntPoint intPoint3 = maxXY - minXY;
|
||||
float num4 = System.Math.Max(minAcceptableDistortion, ((float)intPoint3.X + (float)intPoint3.Y) / 2f * relativeDistortionLimit);
|
||||
return num <= num4;
|
||||
}
|
||||
|
||||
private List<IntPoint> GetShapeCorners(List<IntPoint> edgePoints)
|
||||
{
|
||||
return shapeOptimizer.OptimizeShape(PointsCloud.FindQuadrilateralCorners(edgePoints));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user