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