public static List <MyPoint3D> GetHeadlandIntersections(List <MyPoint3D> heanland, LineDefinition line, double width)
{
List <MyPoint3D> intersections = new List <MyPoint3D>();
if (heanland.Last().X != heanland.First().X || heanland.Last().Y != heanland.First().Y)
{
var original = heanland;
heanland = new List <MyPoint3D>();
foreach (var p in original)
{
heanland.Add(p);
}
heanland.Add(original.First());
}
for (int i = 1; i < heanland.Count; i++)
{
var p1 = heanland[i - 1];
var p2 = heanland[i];
var outerLine = LineDefinition.GetLineDefinition(p1, p2);
var intersection = LineDefinition.GetLineIntersection(outerLine, line);
if (Utility.IsInLineSegment(p1, p2, intersection))
{
intersections.Add(ChooseHeadlandIntersection(heanland, line, width, outerLine, intersection));
}
}
if (intersections.Count % 2 != 0)
{
}
return(intersections);
}
public static List <MyPoint3D> GetPolygonIntersections(List <MyPoint3D> polygon, LineDefinition line)
{
List <MyPoint3D> intersections = new List <MyPoint3D>();
if (polygon.Last().X != polygon.First().X || polygon.Last().Y != polygon.First().Y)
{
var original = polygon;
polygon = new List <MyPoint3D>();
foreach (var p in original)
{
polygon.Add(p);
}
polygon.Add(original.First());
}
for (int i = 1; i < polygon.Count; i++)
{
var p1 = polygon[i - 1];
var p2 = polygon[i];
var intersection = LineDefinition.GetLineIntersection(LineDefinition.GetLineDefinition(p1, p2), line);
if (Utility.IsInLineSegment(p1, p2, intersection))
{
intersections.Add(intersection);
}
}
if (intersections.Count % 2 != 0)
{
}
return(intersections);
}
public static LineDefinition GetParallelLine(LineDefinition line, double distance)
{
if (line.IsVertical)
{
if (line.k == double.PositiveInfinity)
{
return new LineDefinition()
{
IsVertical = true, X = line.X - distance, k = double.PositiveInfinity
}
}
;
else
{
return new LineDefinition()
{
IsVertical = true, X = line.X + distance, k = double.NegativeInfinity
}
};
}
LineDefinition parallelLine = new LineDefinition()
{
k = line.k, b = line.b
};
double alpha = Math.Atan(line.k);
parallelLine.b += distance / Math.Cos(alpha);
return(parallelLine);
}
internal static LineDefinition GetBestLine(List <MyPoint3D> points)
{
LineDefinition bestLine = new LineDefinition();
double[,] A_A = new double[2, 2] {
{ 0, 0 }, { 0, 0 }
};
foreach (var p in points)
{
A_A[0, 0] += p.X * p.X;
A_A[0, 1] += p.X;
}
A_A[1, 0] = A_A[0, 1];
A_A[1, 1] = points.Count;
double overK = 1.0 / (A_A[0, 0] * A_A[1, 1] - A_A[1, 0] * A_A[0, 1]);
double[,] A_minus = new double[2, 2];
A_minus[0, 0] = A_A[1, 1] * overK;
A_minus[0, 1] = -A_A[0, 1] * overK;
A_minus[1, 0] = -A_A[1, 0] * overK;
A_minus[1, 1] = A_A[0, 0] * overK;
foreach (var p in points)
{
bestLine.k += (A_minus[0, 0] * p.X + A_minus[0, 1] * 1) * p.Y;
bestLine.b += (A_minus[1, 0] * p.X + A_minus[1, 1] * 1) * p.Y;
}
return(bestLine);
}
internal static LineDefinition TurnLine(LineDefinition line, double angle, MyPoint3D origin)
{
double alpha = Math.Atan(line.k) + angle;
LineDefinition turnedLine = new LineDefinition()
{
k = Math.Tan(alpha)
};
turnedLine.b = origin.Y - origin.X * turnedLine.k;
return(turnedLine);
}
public static MyPoint3D ShiftPoint(MyPoint3D p1, MyPoint3D p2, MyPoint3D p3, double Length)
{
LineDefinition line1;
if (p1.X > p2.X)
{
line1 = LineDefinition.GetParallelLine(LineDefinition.GetLineDefinition(p1, p2), -Length);
}
else
{
line1 = LineDefinition.GetParallelLine(LineDefinition.GetLineDefinition(p1, p2), Length);
}
LineDefinition line2;
if (p2.X > p3.X)
{
line2 = LineDefinition.GetParallelLine(LineDefinition.GetLineDefinition(p2, p3), -Length);
}
else
{
line2 = LineDefinition.GetParallelLine(LineDefinition.GetLineDefinition(p2, p3), Length);
}
if (line1.IsVertical && line2.IsVertical)
{
MyPoint3D p = new MyPoint3D();
p.Y = p2.Y;
if (p1.Y < p2.Y)
{
p.X = p2.X - Length;
}
else
{
p.X = p2.X + Length;
}
return(p);
}
else if (line1.k == line2.k)
{
return(ShiftPoint(p1, p2, -Length));
}
else
{
return(LineDefinition.GetLineIntersection(line1, line2));
}
}
public static MyPoint3D GetLineIntersection(LineDefinition l1, LineDefinition l2)
{
MyPoint3D point = new MyPoint3D();
if (l1.IsVertical)
{
point.X = l1.X;
point.Y = l2.k * point.X + l2.b;
return(point);
}
if (l2.IsVertical)
{
point.X = l2.X;
point.Y = l1.k * point.X + l1.b;
return(point);
}
point.X = (l2.b - l1.b) / (l1.k - l2.k);
point.Y = point.X * l1.k + l1.b;
return(point);
}
public static LineDefinition GetLineDefinition(MyPoint3D p1, MyPoint3D p2)
{
LineDefinition def = new LineDefinition();
if (p2.X == p1.X)
{
def.IsVertical = true;
def.X = p1.X;
if (p2.Y > p1.Y)
{
def.k = double.PositiveInfinity;
}
else
{
def.k = double.NegativeInfinity;
}
return(def);
}
def.k = (p2.Y - p1.Y) / (p2.X - p1.X);
def.b = p1.Y - def.k * p1.X;
return(def);
}
public static MyPoint3D ChooseHeadlandIntersection(List <MyPoint3D> heanland, LineDefinition line, double width, LineDefinition outerLine, MyPoint3D intersection)
{
double phiOl = Math.Atan(outerLine.k);
double phiL = Math.Atan(line.k);
double alpha = phiOl - phiL;
if (Math.Abs(alpha) < WGS84.Pi / 2)
{
alpha = WGS84.Pi - Math.Abs(alpha);
}
double d = width / 2 / Math.Tan(alpha / 2);
double dx = d * Math.Cos(phiL);
MyPoint3D intersection1 = new MyPoint3D(intersection.X + dx, line.k * (intersection.X + dx) + line.b, 0);
if (IsInsidePolygon(heanland, intersection1))
{
return(intersection1);
}
else
{
MyPoint3D intersection2 = new MyPoint3D(intersection.X - dx, line.k * (intersection.X - dx) + line.b, 0);
return(intersection2);
}
}