public static GeoPoint ToGeo(this MyPoint3D p)
{
double LatitudeDegCoef = WGS84.Pi * WGS84.a / 180.0;
double LongitudeDegCoef = WGS84.Pi * WGS84.a * Math.Cos(LocalLatitude / 180.0 * WGS84.Pi) / 180.0;
GeoPoint geo = new GeoPoint();
geo.Latitude = p.Y / LatitudeDegCoef;
geo.Longitude = p.X / LongitudeDegCoef;
return(geo);
}
public static MyPoint3D ToCart(this GeoPoint p)
{
double LatitudeDegCoef = WGS84.Pi * WGS84.a / 180.0;
double LongitudeDegCoef = WGS84.Pi * WGS84.a * Math.Cos(LocalLatitude / 180.0 * WGS84.Pi) / 180.0;
MyPoint3D cart = new MyPoint3D();
cart.Y = p.Latitude * LatitudeDegCoef;
cart.X = p.Longitude * LongitudeDegCoef;
return(cart);
}
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, double Length)
{
MyPoint3D p = new MyPoint3D();
double gamma = Math.Atan2((p2.Y - p1.Y), (p2.X - p1.X));
double alpha = gamma - WGS84.Pi / 2.0;
double dx = Length * Math.Cos(alpha);
double dy = Length * Math.Sin(alpha);
p.X = p2.X + dx;
p.Y = p2.Y + dy;
return(p);
}
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 bool IsInLineSegment(MyPoint3D p1, MyPoint3D p2, MyPoint3D point)
{
double maxX = Math.Max(p1.X, p2.X) + 0.000001;
double minX = Math.Min(p1.X, p2.X) - 0.000001;
double maxY = Math.Max(p1.Y, p2.Y) + 0.000001;
double minY = Math.Min(p1.Y, p2.Y) - 0.000001;
if (point.X >= minX && point.X <= maxX && point.Y >= minY && point.Y <= maxY)
{
if (point.X == p2.X && point.Y == p2.Y)
{
return(false);
}
return(true);
}
else
{
return(false);
}
}
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);
}
}
public MyPoint3D Normalize()
{
MyPoint3D Vec = new MyPoint3D(X, Y, Z);
return(Vec / this.Norm);
}
public MyPoint3D(MyPoint3D point)
: this(point.X, point.Y, point.Z)
{
}
public static bool IsInsidePolygon(List <MyPoint3D> polygon, MyPoint3D point)
{
if ((polygon.First().X != polygon.Last().X) || (polygon.First().Y != polygon.Last().Y))
{
var inputPoly = polygon;
polygon = new List <MyPoint3D>(inputPoly.Count + 1);
foreach (var p in inputPoly)
{
polygon.Add(p);
}
polygon.Add(inputPoly.First());
}
int IntersectionCounter = 0;
var Pc = point;
for (int i = 1; i < polygon.Count; i++)
{
var p1 = polygon[i - 1];
var p2 = polygon[i];
if (p1.X <= Pc.X)
{
if (p2.X < Pc.X)
{
continue;
}
}
if (p1.X >= Pc.X)
{
if (p2.X > Pc.X)
{
continue;
}
}
if (p1.Y <= Pc.Y)
{
if (p2.Y < Pc.Y)
{
continue;
}
}
if (p1.Y >= Pc.Y)
{
if (p2.Y > Pc.Y)
{
IntersectionCounter++;
continue;
}
}
double k21 = (p2.Y - p1.Y) / (p2.X - p1.X);
double kc1 = (Pc.Y - p1.Y) / (Pc.X - p1.X);
if (p2.X > p1.X)
{
if (kc1 <= k21)
{
IntersectionCounter++;
}
}
else
{
if (kc1 >= k21)
{
IntersectionCounter++;
}
}
}
if (IntersectionCounter % 2 == 0) // is even
{
return(false);
}
else
{
return(true);
}
}