public static double SpatialDistanceBetweenPlaces(Wgs84Coordinates a, Wgs84Coordinates b)
{
var fablat = new DotSpatial.Positioning.Latitude((double)a.Latitude);
var fablng = new DotSpatial.Positioning.Longitude((double)a.Longitude);
var sglat = new DotSpatial.Positioning.Latitude((double)b.Latitude);
var sglng = new DotSpatial.Positioning.Longitude((double)b.Longitude);
var fab = new DotSpatial.Positioning.Position(fablat, fablng);
var sg = new DotSpatial.Positioning.Position(sglat, sglng);
DotSpatial.Positioning.Distance dist = fab.DistanceTo(sg);
return(dist.ToMeters().Value);
} // End Function SpatialDistanceBetweenPlaces
} // End Function SpatialDistanceBetweenPlaces
public static double SpatialDistanceBetweenPlacesDotSpatial(
double lat1
, double lng1
, double lat2
, double lng2)
{
var fablat = new DotSpatial.Positioning.Latitude(lat1);
var fablng = new DotSpatial.Positioning.Longitude(lng1);
var sglat = new DotSpatial.Positioning.Latitude(lat2);
var sglng = new DotSpatial.Positioning.Longitude(lng2);
var fab = new DotSpatial.Positioning.Position(fablat, fablng);
var sg = new DotSpatial.Positioning.Position(sglat, sglng);
DotSpatial.Positioning.Distance dist = fab.DistanceTo(sg);
return(dist.ToMeters().Value);
} // End Function SpatialDistanceBetweenPlacesDotSpatial
/// <summary>
/// If GGA NMEA data is available, calculate the magnitude and direction
/// of the current position to the start.
/// </summary>
/// <param name="ensemble"></param>
private void AccumulateGps(DataSet.Ensemble ensemble)
{
// If the previous postion has not been set, we cannot calculate yet
if (_firstGpsPos == DotSpatial.Positioning.Position.Empty)
{
if (ensemble.IsNmeaAvail)
{
if (ensemble.NmeaData.IsGpggaAvail() && !ensemble.NmeaData.GPGGA.Position.IsInvalid && ensemble.NmeaData.GPGGA.Position.Longitude.DecimalDegrees != Double.NaN)
{
_firstGpsPos = ensemble.NmeaData.GPGGA.Position;
GpsPoints.Points.Add(new DataPoint(0 + XOffset, 0 + YOffset));
}
}
// Nothing to calculate yet
return;
}
// Calculate the magnitude and direction
// Use the first position and the current position
// If the previous postion has not been set, we cannot calculate yet
if (_firstGpsPos != DotSpatial.Positioning.Position.Empty && !_firstGpsPos.IsInvalid)
{
if (ensemble.IsNmeaAvail)
{
if (ensemble.NmeaData.IsGpggaAvail() && !ensemble.NmeaData.GPGGA.Position.IsInvalid && ensemble.NmeaData.GPGGA.Position.Longitude.DecimalDegrees != Double.NaN)
{
GpsMag = _firstGpsPos.DistanceTo(ensemble.NmeaData.GPGGA.Position).ToMeters().Value; // Meters Distance Made Good
GpsDir = _firstGpsPos.BearingTo(ensemble.NmeaData.GPGGA.Position).DecimalDegrees; // Decimal Degrees Course Made Good
//GpsDir = ensemble.NmeaData.GPGGA.Position.BearingTo(_firstGpsPos).DecimalDegrees; // Decimal Degrees Course Over Ground
// Generate X,Y point
double x = YOffset + (GpsMag * Math.Sin(MathHelper.DegreeToRadian(GpsDir)));
double y = XOffset + (GpsMag * Math.Cos(MathHelper.DegreeToRadian(GpsDir)));
// Add the point to the line series
GpsPoints.Points.Add(new DataPoint(x, y));
}
}
}
}
/// <summary>
/// Initialize all the values.
/// </summary>
private void InitValues()
{
_firstGpsPos = DotSpatial.Positioning.Position.Empty;
GpsDir = 0.0;
GpsMag = 0.0;
GpsPoints = new LineSeries()
{
Color = GpsColor, StrokeThickness = 1, Title = "GPS"
};
BtEarthDir = 0.0;
BtEarthMag = 0.0;
BtEarthPoints = new LineSeries()
{
Color = BtEarthColor, StrokeThickness = 1, Title = "BT ENU"
};
BtE = 0.0;
BtN = 0.0;
BtU = 0.0;
_prevBtEarthTime = -1.0f;
_prevBtE = 0.0f;
_prevBtN = 0.0f;
_prevBtU = 0.0f;
BtInstrumentDir = 0.0;
BtInstrumentMag = 0.0;
BtInstrumentPoints = new LineSeries()
{
Color = BtInstrumentColor, StrokeThickness = 1, Title = "BT XYZ"
};
BtX = 0.0;
BtY = 0.0;
BtZ = 0.0;
_prevBtInstrumentTime = -1.0f;
_prevBtX = 0.0f;
_prevBtY = 0.0f;
_prevBtZ = 0.0f;
WpEarthDir = 0.0;
WpEarthMag = 0.0;
WpEarthPoints = new LineSeries()
{
Color = WpEarthColor, StrokeThickness = 1, Title = "WP ENU"
};
WpE = 0.0;
WpN = 0.0;
WpU = 0.0;
_prevWpEarthTime = -1.0f;
_prevWpE = 0.0f;
_prevWpN = 0.0f;
_prevWpU = 0.0f;
WpInstrumentDir = 0.0;
WpInstrumentMag = 0.0;
WpInstrumentPoints = new LineSeries()
{
Color = WpInstrumentColor, StrokeThickness = 1, Title = "WP XYZ"
};
WpX = 0.0;
WpY = 0.0;
WpZ = 0.0;
_prevWpInstrumentTime = -1.0f;
_prevWpX = 0.0f;
_prevWpY = 0.0f;
_prevWpZ = 0.0f;
BtEarthPercentError = 0;
BtInstrumentPercentError = 0;
WpEarthPercentError = 0;
WpInstrumentPercentError = 0;
BtEarthDirError = 0;
BtInstrumentDirError = 0;
WpEarthDirError = 0;
WpInstrumentDirError = 0;
}
