public SbetRecord GetClosestRecord(DateTime pTime) { DateTime time = pTime; SbetRecord lowKey = null, highKey = null; foreach (KeyValuePair <DateTime, SbetRecord> key in Records) { if (key.Key.Ticks > time.Ticks) { highKey = key.Value; break; } lowKey = key.Value; } SbetRecord rec; if (time.Ticks - lowKey.utcTime.Ticks > highKey.utcTime.Ticks - time.Ticks) { rec = highKey; } else { rec = lowKey; } return(rec); }
private void Load() { SortedList <DateTime, string> camEvents = new SortedList <DateTime, string>(); if (!string.IsNullOrEmpty(EventsList)) { //Load the camera events Parallel.ForEach(File.ReadAllLines(EventsList), e => { List <string> temp = e.Split().ToList(); temp.RemoveAll(str => String.IsNullOrEmpty(str)); lock (camEvents) { camEvents.Add(GetFromGps(GPSWeek, Double.Parse(temp[0])), temp[1]); }; }); } List <double> vals = new List <double>(); // Open an SBET file for reading... using (BinaryReader reader = new BinaryReader(File.Open(LedgerPath, FileMode.Open, FileAccess.Read))) { int length = (int)reader.BaseStream.Length; while (reader.BaseStream.Position != length) { vals.Add(reader.ReadDouble()); } } SortedList <DateTime, int> indexes = new SortedList <DateTime, int>(); for (int i = 0; i < vals.Count; i += 17) { indexes.Add(GetFromGps(GPSWeek, vals[i]), i); } //if no list of camevents is provided, just add all the records, otherwise only add the closest SBet records. if (camEvents.Count > 0) { //Slow bit SortedList <DateTime, int> newIndexes = new SortedList <DateTime, int>(); Parallel.ForEach(camEvents, (evnt) => { lock (newIndexes) { newIndexes.Add(GetClosestKey(evnt.Key, indexes).Key, GetClosestKey(evnt.Key, indexes).Value); } }); indexes = newIndexes; } LatLngUTMConverter converter = new LatLngUTMConverter("WGS 84"); Parallel.ForEach(indexes, (i) => { DateTime tempUtcTime = i.Key; double eastng = 0.0; double northng = 0.0; double height = vals[i.Value + 3]; if (CoordSyst == "UTM") { LatLngUTMConverter.UTMResult res = converter.ConvertLatLngToUtm((vals[i.Value + 1] * 180.0) / Math.PI, (vals[i.Value + 2] * 180.0) / Math.PI); eastng = res.Easting; northng = res.Northing; } else { Console.WriteLine("No coordinate system defined"); } SbetRecord rec = new SbetRecord { //Not required here //time = vals[i], utcTime = tempUtcTime, //Not required here //latRad = vals[i + 1], //Not required here //lonRad = vals[i + 2], //Not required here //alt = height, XYZ = new Point3D(eastng, northng, height), //Not required here //xVel = vals[i + 4], //Not required here //yVel = vals[i + 5], //Not required here //zVel = vals[i + 6], roll = vals[i.Value + 7], pitch = vals[i.Value + 8], yaw = vals[i.Value + 9], //Not required here //headingDeg = RadianToDegree(vals[i + 9]), //Not required here //wander = vals[i + 10], //Not required here //xForce = vals[i + 11], //Not required here //yForce = vals[i + 12], //Not required here //zForce = vals[i + 13], //Not required here //xAngRate = vals[i + 14], //Not required here //yAngRate = vals[i + 15], //Not required here //zAngRate = vals[i + 16] }; lock (Records) { Records.Add(rec.utcTime, rec); } }); }