private static IEnumerable <AXPoint> InterpolateGapSpline(AXPoint p0, AXPoint p1, AXPoint p2, AXPoint p3, int interpolationInterval, int maxAllowedGap) { // calculate the number of points to be interpolated var numberOfPoints = ((int)Math.Floor((p2.Time - p1.Time).TotalSeconds / interpolationInterval)) - 1; // don't interpolate if it's not needed or the gap is too large if (numberOfPoints > 0 && numberOfPoints <= maxAllowedGap) { // define interpolator // "convert" time to double var x0 = 0; var x1 = (p1.Time - p0.Time).TotalSeconds; var x2 = (p2.Time - p0.Time).TotalSeconds; var x3 = (p3.Time - p0.Time).TotalSeconds; var interpolator = CubicInterpolator.CatmullRom(x0, x1, x2, x3); var deltat = 1.0 / (numberOfPoints + 1); for (int i = 1; i <= numberOfPoints; i++) { // perform interpolation var p = new AXPoint( p1.Time.AddSeconds(i * interpolationInterval), interpolator.Interpolate(p0.Easting, p1.Easting, p2.Easting, p3.Easting, i * deltat), interpolator.Interpolate(p0.Northing, p1.Northing, p2.Northing, p3.Northing, i * deltat), interpolator.Interpolate(p0.Altitude, p1.Altitude, p2.Altitude, p3.Altitude, i * deltat)); yield return(p); } } }
// implements track log linear interpolation public static IEnumerable <AXPoint> Linear(IEnumerable <AXPoint> track, int interpolationInterval = 1, int maxAllowedGap = 10) { AXPoint p0 = null; foreach (var p1 in track) { if (p0 != null) { // calculate the number of points to be interpolated var numberOfPoints = ((int)Math.Floor((p1.Time - p0.Time).TotalSeconds / interpolationInterval)) - 1; // don't interpolate if it's not needed or the gap is too large if (numberOfPoints > 0 && numberOfPoints <= maxAllowedGap) { var deltat = 1.0 / (numberOfPoints + 1); for (int i = 1; i <= numberOfPoints; i++) { // perform interpolation var p = new AXPoint( p0.Time.AddSeconds(i * interpolationInterval), InterpolateSingleLinear(p0.Easting, p1.Easting, i * deltat), InterpolateSingleLinear(p0.Northing, p1.Northing, i * deltat), InterpolateSingleLinear(p0.Altitude, p1.Altitude, i * deltat)); yield return(p); } } } yield return(p1); p0 = p1; } }
// implements track log cubic hermite spline interpolation with Catmull-Rom tension public static IEnumerable <AXPoint> Spline(IEnumerable <AXPoint> track, int interpolationInterval = 1, int maxAllowedGap = 10) { var newTrack = new List <AXPoint>(); AXPoint p0 = null; AXPoint p1 = null; AXPoint p2 = null; foreach (var p3 in track) { if (p2 == null) { p0 = p1 = p2 = p3; } else { if (p1 != p2) { newTrack.AddRange(InterpolateGapSpline(p0, p1, p2, p3, interpolationInterval, maxAllowedGap)); } newTrack.Add(p2); } p0 = p1; p1 = p2; p2 = p3; } //last point newTrack.AddRange(InterpolateGapSpline(p0, p1, p2, p2, interpolationInterval, maxAllowedGap)); newTrack.Add(p2); return(newTrack.ToArray()); }
public static double DistanceRad(AXPoint point1, AXPoint point2, double radTreshold) { if (Math.Abs(point1.Altitude - point2.Altitude) <= radTreshold) { return(Distance2D(point1, point2)); } else { return(Distance3D(point1, point2)); } }
/// <summary>Area of a triangle given the three vertices /// Heron's formula: Area = SQRT(s(s-a)(s-b)(s-c)) where s is the semiperimeter=(a+b+c)/2 /// </summary> /// <param name="point1">First vertex</param> /// <param name="point2">Second vertex</param> /// <param name="point3">Third vertex</param> /// <returns>Area of the triangle in m2</returns> public static double Area(AXPoint point1, AXPoint point2, AXPoint point3) { double sideA, sideB, sideC, semiPerimeter; sideA = Distance2D(point1, point2); sideB = Distance2D(point2, point3); sideC = Distance2D(point3, point1); semiPerimeter = (sideA + sideB + sideC) / 2; return(Math.Sqrt(semiPerimeter * (semiPerimeter - sideA) * (semiPerimeter - sideB) * (semiPerimeter - sideC))); }
public bool Contains(AXPoint p) { foreach (var s in segments) { if (s[0].Time <= p.Time && p.Time <= s[s.Length - 1].Time) { return(true); } } return(false); }
public double Distance2D() { var dist = 0.0; foreach (var s in segments) { AXPoint previous = null; foreach (var p in s) { if (previous != null) { dist += Physics.Distance2D(previous, p); } previous = p; } } return(dist); }
public static TimeSpan TimeDiff(AXPoint point1, AXPoint point2) { return(point2.Time - point1.Time); }
/// <summary> /// Computes the direction from the second point to the first. 0 is grid north. /// </summary> /// <param Name="point1">First point</param> /// <param Name="point2">Second point</param> /// <returns>Direction in degrees</returns> public static double Direction2D(AXPoint point1, AXPoint point2) { return(90 - 180 * Math.Atan2(point2.Northing - point1.Northing, point2.Easting - point1.Easting) / Math.PI); }
public static double Acceleration3D(AXPoint point1, AXPoint point2, AXPoint point3) { return((Velocity2D(point2, point3) - Velocity3D(point1, point2)) / TimeDiff(point1, point3).TotalSeconds); }
public static double VerticalVelocity(AXPoint point1, AXPoint point2) { return((point2.Altitude - point1.Altitude) / TimeDiff(point1, point2).TotalSeconds); }
public static double Velocity3D(AXPoint point1, AXPoint point2) { return(Distance3D(point1, point2) / TimeDiff(point1, point2).TotalSeconds); }
public static double Distance3D(AXPoint point1, AXPoint point2) { return(Math.Sqrt(Math.Pow(point1.Easting - point2.Easting, 2) + Math.Pow(point1.Northing - point2.Northing, 2) + Math.Pow(point1.Altitude - point2.Altitude, 2))); }
public AXWaypoint(string name, AXPoint point) : this(name, point.Time, point.Easting, point.Northing, point.Altitude) { }