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)
{
}