public void Trace(GeoData raw, GeoData corrected, GpsStatus gpsStatus, DateTime computerDateTime, RoutepointExtension rteptExtension)
{
if (raw == null)
{
Log.Warn().Message("raw est null").Write(); return;
}
if (corrected == null)
{
Log.Warn().Message("corrected est null").Write(); return;
}
if (raw.PositionData == null)
{
Log.Warn().Message("raw.PositionData est null").Write(); return;
}
if (raw.VelocityData == null)
{
Log.Warn().Message("raw.VelocityData est null").Write(); return;
}
if (corrected.PositionData == null)
{
Log.Warn().Message("corrected.PositionData est null").Write(); return;
}
if (corrected.VelocityData == null)
{
Log.Warn().Message("corrected.VelocityData est null").Write(); return;
}
if (_output != null && (_output.WriteState == WriteState.Closed || _output.WriteState == WriteState.Error))
{
Log.Warn().Message("Le fichier de trace est fermé ou en erreur.").Write(); return;
}
_output.WriteStartElement("rtept");
{
_output.WriteAttributeString("lat", Convert.ToString(raw.PositionData.Latitude, CultureInfo.InvariantCulture));
_output.WriteAttributeString("lon", Convert.ToString(raw.PositionData.Longitude, CultureInfo.InvariantCulture));
_output.WriteElementString("time", raw.PositionData.Utc.ToString("yyyy-MM-ddTHH:mm:ss.ffK"));
_output.WriteElementString("ele", Convert.ToString(raw.PositionData.Altitude, CultureInfo.InvariantCulture));
_output.WriteElementString("geoidheight", Convert.ToString(raw.PositionData.GeoIdHeight, CultureInfo.InvariantCulture));
_output.WriteElementString("fix", GpxConvert.FromFixType(raw.PositionData.Quality));
_output.WriteElementString("sat", Convert.ToString(raw.PositionData.NbSatellites, CultureInfo.InvariantCulture));
if (raw.PrecisionData != null)
{
if (raw.PrecisionData.Hdop != null)
{
_output.WriteElementString("hdop", Convert.ToString(raw.PrecisionData.Hdop, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.Vdop != null)
{
_output.WriteElementString("vdop", Convert.ToString(raw.PrecisionData.Vdop, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.Pdop != null)
{
_output.WriteElementString("pdop", Convert.ToString(raw.PrecisionData.Pdop, CultureInfo.InvariantCulture));
}
}
_output.WriteElementString("ageofdgpsdata", Convert.ToString(raw.PositionData.DifferentialDataAge, CultureInfo.InvariantCulture));
_output.WriteElementString("dgpsid", Convert.ToString(raw.PositionData.DifferentialStationId));
_output.WriteStartElement("extensions");
{
_output.WriteElementString("computerDateTime", computerDateTime.ToString("yyyy-MM-ddTHH:mm:ss.fffffffK"));
_output.WriteStartElement("CorrectedPositionValue");
{
_output.WriteElementString("lat", Convert.ToString(corrected.PositionData.Latitude, CultureInfo.InvariantCulture));
_output.WriteElementString("lon", Convert.ToString(corrected.PositionData.Longitude, CultureInfo.InvariantCulture));
}
_output.WriteEndElement();
_output.WriteElementString("gpsStatus", Convert.ToString(gpsStatus));
_output.WriteElementString("speedKmh", Convert.ToString(raw.VelocityData.SpeedKmh, CultureInfo.InvariantCulture));
if (rteptExtension != null)
{
if (rteptExtension.Progress.HasValue)
{
_output.WriteElementString("progress", Convert.ToString(rteptExtension.Progress.Value, CultureInfo.InvariantCulture));
}
}
if (raw.PositionData.InsData != null)
{
_output.WriteStartElement("ins", "inertial", null);
{
_output.WriteElementString("navigationStatus", Convert.ToString(raw.PositionData.InsData.Status, CultureInfo.InvariantCulture));
_output.WriteElementString("accx", Convert.ToString(raw.PositionData.InsData.AccelerationX, CultureInfo.InvariantCulture));
_output.WriteElementString("accy", Convert.ToString(raw.PositionData.InsData.AccelerationY, CultureInfo.InvariantCulture));
_output.WriteElementString("accz", Convert.ToString(raw.PositionData.InsData.AccelerationZ, CultureInfo.InvariantCulture));
_output.WriteElementString("angrtx", Convert.ToString(raw.PositionData.InsData.AngularRateX, CultureInfo.InvariantCulture));
_output.WriteElementString("angrty", Convert.ToString(raw.PositionData.InsData.AngularRateY, CultureInfo.InvariantCulture));
_output.WriteElementString("angrtz", Convert.ToString(raw.PositionData.InsData.AngularRateZ, CultureInfo.InvariantCulture));
_output.WriteElementString("nvelo", Convert.ToString(raw.PositionData.InsData.NorthVelocity, CultureInfo.InvariantCulture));
_output.WriteElementString("evelo", Convert.ToString(raw.PositionData.InsData.EastVelocity, CultureInfo.InvariantCulture));
_output.WriteElementString("dvelo", Convert.ToString(raw.PositionData.InsData.DownVelocity, CultureInfo.InvariantCulture));
_output.WriteElementString("heading", Convert.ToString(raw.PositionData.InsData.Heading, CultureInfo.InvariantCulture));
_output.WriteElementString("pitch", Convert.ToString(raw.PositionData.InsData.Pitch, CultureInfo.InvariantCulture));
_output.WriteElementString("roll", Convert.ToString(raw.PositionData.InsData.Roll, CultureInfo.InvariantCulture));
if (raw.PrecisionData != null)
{
if (raw.PrecisionData.EastPositionAccuracy != null)
{
_output.WriteElementString("eastPositionAccuracy", Convert.ToString(raw.PrecisionData.EastPositionAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.DownPositionAccuracy != null)
{
_output.WriteElementString("downPositionAccuracy", Convert.ToString(raw.PrecisionData.DownPositionAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.NorthPositionAccuracy != null)
{
_output.WriteElementString("northPositionAccuracy", Convert.ToString(raw.PrecisionData.NorthPositionAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.DownVelocityAccuracy != null)
{
_output.WriteElementString("downVelocityAccuracy", Convert.ToString(raw.PrecisionData.DownVelocityAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.EastVelocityAccuracy != null)
{
_output.WriteElementString("eastVelocityAccuracy", Convert.ToString(raw.PrecisionData.EastVelocityAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.NorthVelocityAccuracy != null)
{
_output.WriteElementString("northVelocityAccuracy", Convert.ToString(raw.PrecisionData.NorthVelocityAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.HeadingAccuracy != null)
{
_output.WriteElementString("headingAccuracy", Convert.ToString(raw.PrecisionData.HeadingAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.PitchAccuracy != null)
{
_output.WriteElementString("pitchAccuracy", Convert.ToString(raw.PrecisionData.PitchAccuracy, CultureInfo.InvariantCulture));
}
if (raw.PrecisionData.RollAccuracy != null)
{
_output.WriteElementString("rollAccuracy", Convert.ToString(raw.PrecisionData.RollAccuracy, CultureInfo.InvariantCulture));
}
}
}
_output.WriteEndElement();
}
}
_output.WriteEndElement();
}
_output.WriteEndElement();
}
private static double DistanceBetweenVehicleAndBGrVirtualLine(GeoCoordinate triggerCoordinate, GeoData currentCoord, int frequency, Vector <double> directionalTriggerVector)
{
Vector <double> bgrTriggerVector = ConvertGeoCoordinateToCartesian(triggerCoordinate.Latitude, triggerCoordinate.Longitude);
Vector <double> inversedBGRDirectionalTriggerVector = directionalTriggerVector * -1;
inversedBGRDirectionalTriggerVector = inversedBGRDirectionalTriggerVector.Normalize(2);
Vector <double> currentVehicleVector = ConvertGeoCoordinateToCartesian(currentCoord.PositionData.Latitude, currentCoord.PositionData.Longitude);
// calculate distance between the vehicle and a perpendicular line to the provided directional vector
Vector <double> triggerPointToVehicleVector = currentVehicleVector.Subtract(bgrTriggerVector);
triggerPointToVehicleVector = triggerPointToVehicleVector.Normalize(2);
double radAngle =
Math.Acos(
inversedBGRDirectionalTriggerVector.DotProduct(triggerPointToVehicleVector)
/ (
Math.Sqrt(inversedBGRDirectionalTriggerVector.DotProduct(inversedBGRDirectionalTriggerVector))
* Math.Sqrt(triggerPointToVehicleVector.DotProduct(triggerPointToVehicleVector))
)
);
var orthodromicDistance = OrthodromicDistance(currentCoord.PositionData.Latitude, currentCoord.PositionData.Longitude, triggerCoordinate.Latitude, triggerCoordinate.Longitude);
double distanceVehicleToVirtualTriggerLine = Math.Cos(radAngle) * orthodromicDistance;
double averageDistanceBetweenGpsCoordinates = CalculateAverageDistanceBySpeed(currentCoord.VelocityData.SpeedKmh, frequency);
return(distanceVehicleToVirtualTriggerLine - averageDistanceBetweenGpsCoordinates);
}
/// <summary>
/// Traces the position data.
/// </summary>
/// <remarks>Writes the output in GPX format.</remarks>
public void Trace(GeoData raw, GeoData corrected, GpsStatus gpsStatus, DateTime computerDateTime)
{
Trace(raw, corrected, gpsStatus, computerDateTime, null);
}
