/// <summary>
/// Finds the closest point on the polygon from a given point
/// </summary>
/// <param name="points">The points that represent a polygon.</param>
/// <param name="p">The point from which the closest point should be found.</param>
/// <param name="closed">if set to <c>true</c> the polygon is closed.</param>
/// <returns>UV.</returns>
public static UV ClosestPoint(UV[] points, UV p, bool closed)
{
UV[] vecs = new UV[points.Length];
double[] vecLengths = new double[points.Length];
for (int i = 0; i < points.Length; i++)
{
vecs[i] = points[i] - p;
vecLengths[i] = vecs[i].GetLengthSquared();
}
double minDistSquared = double.PositiveInfinity;
int index = 0;
for (int i = 0; i < points.Length - 1; i++)
{
double area = vecs[i].CrossProductValue(vecs[i + 1]);
area *= area;
double d1 = UV.GetLengthSquared(points[i], points[i + 1]);
double distSquared = area / d1;
if (distSquared < vecLengths[i] - d1 || distSquared < vecLengths[i + 1] - d1)
{
distSquared = Math.Min(vecLengths[i + 1], vecLengths[i]);
}
if (minDistSquared > distSquared)
{
minDistSquared = distSquared;
index = i;
}
}
if (closed)
{
double area = vecs[points.Length - 1].CrossProductValue(vecs[0]);
area *= area;
double d1 = UV.GetLengthSquared(points[points.Length - 1], points[0]);
double distSquared = area / d1;
if (distSquared < vecLengths[0] - d1 || distSquared < vecLengths[points.Length - 1] - d1)
{
distSquared = Math.Min(vecLengths[0], vecLengths[points.Length - 1]);
}
if (minDistSquared > distSquared)
{
minDistSquared = distSquared;
index = points.Length - 1;
}
}
int j = index + 1;
if (j == points.Length)
{
j = 0;
}
UVLine line = new UVLine(points[index], points[j]);
return(p.GetClosestPoint(line));
}
/// <summary>
/// Returns the distance squared to a list of points that represent a polygon
/// </summary>
/// <param name="pLinePoints">A list of points that represent a polygon.</param>
/// <param name="p">The point to measure the distance squared from.</param>
/// <param name="closed">if set to <c>true</c> the polygon is closed.</param>
/// <returns>System.Double.</returns>
public static double DistanceSquaredTo(UV[] pLinePoints, UV p, bool closed)
{
UV[] vecs = new UV[pLinePoints.Length];
double[] vecLengths = new double[pLinePoints.Length];
for (int i = 0; i < pLinePoints.Length; i++)
{
vecs[i] = pLinePoints[i] - p;
vecLengths[i] = vecs[i].GetLengthSquared();
}
double minDistSquared = double.PositiveInfinity;
int index = 0;
for (int i = 0; i < pLinePoints.Length - 1; i++)
{
double area = vecs[i].CrossProductValue(vecs[i + 1]);
area *= area;
double d1 = UV.GetLengthSquared(pLinePoints[i], pLinePoints[i + 1]);
double distSquared = area / d1;
if (distSquared < vecLengths[i] - d1 || distSquared < vecLengths[i + 1] - d1)
{
distSquared = Math.Min(vecLengths[i + 1], vecLengths[i]);
}
if (minDistSquared > distSquared)
{
minDistSquared = distSquared;
index = i;
}
}
if (closed)
{
double area = vecs[pLinePoints.Length - 1].CrossProductValue(vecs[0]);
area *= area;
double d1 = UV.GetLengthSquared(pLinePoints[pLinePoints.Length - 1], pLinePoints[0]);
double distSquared = area / d1;
if (distSquared < vecLengths[0] - d1 || distSquared < vecLengths[pLinePoints.Length - 1] - d1)
{
distSquared = Math.Min(vecLengths[0], vecLengths[pLinePoints.Length - 1]);
}
if (minDistSquared > distSquared)
{
minDistSquared = distSquared;
index = pLinePoints.Length - 1;
}
}
return(minDistSquared);
}
/// <summary>
/// Loads the factors.
/// </summary>
public void LoadFactors()
{
UV v1 = new UV();
UV v2 = new UV();
int i = 0;
foreach (var item in this.Connections)
{
if (i == 2)
{
break;
}
else if (i == 0)
{
v1 = this.Point - item.Point;
}
if (i == 1)
{
v2 = this.Point - item.Point;
}
i++;
}
if (i < 2)
{
this.AngleFacotr = double.PositiveInfinity;
this.DistanceFactor = double.PositiveInfinity;
}
else
{
double d1 = v1.GetLengthSquared();
double d2 = v2.GetLengthSquared();
this.DistanceFactor = Math.Min(d1, d2);
double crossProductValue = v1.CrossProductValue(v2);
this.AngleFacotr = crossProductValue * crossProductValue / (d1 * d2);
}
}
/// <summary>
/// Gets the length squared of this line.
/// </summary>
/// <returns>System.Double.</returns>
public double GetLengthSquared()
{
return(UV.GetLengthSquared(this.Start, this.End));
}
/// <summary>
/// Gets the squared distance between two states.
/// </summary>
/// <param name="state1">The state1.</param>
/// <param name="state2">The state2.</param>
/// <returns>System.Double.</returns>
public static double DistanceSquared(StateBase state1, StateBase state2)
{
return(UV.GetLengthSquared(state1.Direction, state2.Direction) + UV.GetLengthSquared(state1.Location, state2.Location) + UV.GetLengthSquared(state1.Velocity, state2.Velocity));
}