public static Kml AsKml(this IList <Coordinate> coordinates)
{
var top = coordinates.Max(q => q.Z);
var kml = new Kml
{
Feature = new Document()
};
for (var i = 0; i < coordinates.Count() - 1; i++)
{
LineString ls = new LineString
{
AltitudeMode = AltitudeMode.Absolute,
Extrude = false
};
ls.Coordinates = new CoordinateCollection();
// I guess KML (and Google Earth) works with an altitude in meters. So therefore the 0.3048.
ls.Coordinates.Add(new Vector(coordinates[i].Y, coordinates[i].X, coordinates[i].Z * 0.3048));
ls.Coordinates.Add(new Vector(coordinates[i + 1].Y, coordinates[i + 1].X, coordinates[i + 1].Z * 0.3048));
var placemark = new Placemark
{
Geometry = ls
};
placemark.AddStyle(new Style
{
Line = new LineStyle
{
Color = GetColor(coordinates[i].Z / top)
}
});
kml.Feature.AddChild(placemark);
}
return(kml);
}
public void writeKML(string filename)
{
try
{
writeGPX(filename);
}
catch
{ }
try
{
writeRinex(filename);
}
catch
{ }
try
{
writeWPFile(filename);
}
catch
{ }
try
{
writeParamFile(filename);
}
catch
{ }
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
AltitudeMode altmode = AltitudeMode.absolute;
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4));
Style style1 = new Style();
style1.Id = "spray";
style1.Add(new LineStyle(HexStringToColor("4c0000ff"), 0));
style1.Add(new PolyStyle()
{
Color = HexStringToColor("4c0000ff")
});
PolyStyle pstyle = new PolyStyle();
pstyle.Color = HexStringToColor("7f00ff00");
style.Add(pstyle);
kml.Document.AddStyle(style);
kml.Document.AddStyle(style1);
int stylecode = 0xff;
int g = -1;
foreach (List <Point3D> poslist in position)
{
g++;
if (poslist == null)
{
continue;
}
/*
* List<PointLatLngAlt> pllalist = new List<PointLatLngAlt>();
*
* foreach (var point in poslist)
* {
* pllalist.Add(new PointLatLngAlt(point.Y, point.X, point.Z, ""));
* }
*
* var ans = Utilities.LineOffset.GetPolygon(pllalist, 2);
*
*
*
* while (ans.Count > 0)
* {
* var first = ans[0];
* var second = ans[1];
* var secondlast = ans[ans.Count - 2];
* var last = ans[ans.Count-1];
*
* ans.Remove(first);
* ans.Remove(last);
*
* var polycoords = new BoundaryIs();
*
* polycoords.LinearRing = new LinearRing();
*
* polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(second.Lng, second.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(secondlast.Lng, secondlast.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(last.Lng, last.Lat, 1));
* polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1));
*
* //if (!IsClockwise(polycoords.LinearRing.Coordinates))
* // polycoords.LinearRing.Coordinates.Reverse();
*
* Polygon kmlpoly = new Polygon() { AltitudeMode = AltitudeMode.relativeToGround, Extrude = false, OuterBoundaryIs = polycoords };
*
* Placemark pmpoly = new Placemark();
* pmpoly.Polygon = kmlpoly;
* pmpoly.name = g + " test";
* pmpoly.styleUrl = "#spray";
*
* fldr.Add(pmpoly);
* }
*/
LineString ls = new LineString();
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
Coordinates coords = new Coordinates();
coords.AddRange(poslist);
ls.coordinates = coords;
Placemark pm = new Placemark();
string mode = "";
if (g < modelist.Count)
{
mode = modelist[g];
}
pm.name = g + " Flight Path " + mode;
pm.styleUrl = "#yellowLineGreenPoly";
pm.LineString = ls;
stylecode = colours[g % (colours.Length - 1)].ToArgb();
Style style2 = new Style();
Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff,
(stylecode >> 0) & 0xff);
log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString());
style2.Add(new LineStyle(color, 4));
pm.AddStyle(style2);
fldr.Add(pm);
}
Placemark pmPOS = new Placemark();
pmPOS.name = "POS Message";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
Point3D lastPoint3D = new Point3D();
PointLatLngAlt lastplla = PointLatLngAlt.Zero;
foreach (var item in PosLatLngAlts)
{
var newpoint = new Point3D(item.Lng, item.Lat, item.Alt);
if (item.GetDistance(lastplla) < 0.1 &&
lastPoint3D.Z >= (newpoint.Z - 0.3) &&
lastPoint3D.Z <= (newpoint.Z + 0.3))
{
continue;
}
pmPOS.LineString.coordinates.Add(newpoint);
lastPoint3D = newpoint;
lastplla = item;
if (pmPOS.LineString.coordinates.Count > 20000)
{
//add current
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
// create new
pmPOS = new Placemark();
pmPOS.name = "POS Message - extra";
pmPOS.LineString = new LineString();
pmPOS.LineString.coordinates = new Coordinates();
lastPoint3D = new Point3D();
lastplla = PointLatLngAlt.Zero;
}
}
pmPOS.AddStyle(style);
fldr.Add(pmPOS);
Folder planes = new Folder();
planes.name = "Planes";
fldr.Add(planes);
Folder waypoints = new Folder();
waypoints.name = "Waypoints";
fldr.Add(waypoints);
LineString lswp = new LineString();
lswp.AltitudeMode = AltitudeMode.relativeToGround;
lswp.Extrude = true;
Coordinates coordswp = new Coordinates();
foreach (PointLatLngAlt p1 in cmd)
{
if (p1.Lng == 0 && p1.Lat == 0)
{
continue;
}
coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt));
}
lswp.coordinates = coordswp;
Placemark pmwp = new Placemark();
pmwp.name = "Waypoints";
//pm.styleUrl = "#yellowLineGreenPoly";
pmwp.LineString = lswp;
if (coordswp.Count > 0)
{
waypoints.Add(pmwp);
}
int a = 0;
int l = -1;
Model lastmodel = null;
foreach (Data mod in flightdata)
{
l++;
if (mod.model.Location.latitude == 0)
{
continue;
}
if (lastmodel != null)
{
if (lastmodel.Location.Equals(mod.model.Location))
{
continue;
}
}
Placemark pmplane = new Placemark();
pmplane.name = "Plane " + a;
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
try
{
pmplane.description = @"<![CDATA[
<table>
<tr><td>Roll: " + model.Orientation.roll + @" </td></tr>
<tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr>
<tr><td>Yaw: " + model.Orientation.heading + @" </td></tr>
<tr><td>WP dist " + mod.ntun[2] + @" </td></tr>
<tr><td>tar bear " + mod.ntun[3] + @" </td></tr>
<tr><td>nav bear " + mod.ntun[4] + @" </td></tr>
<tr><td>alt error " + mod.ntun[5] + @" </td></tr>
</table>
]]>";
}
catch
{
}
try
{
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude,
(float)model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";
model.Link = link;
pmplane.Model = model;
planes.Add(pmplane);
}
catch
{
} // bad lat long value
lastmodel = mod.model;
a++;
}
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.ToLower().Replace(".log.kml", ".kmz").Replace(".bin.kml", ".kmz"),
FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
// entry 1
string entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
File.Delete(filename);
filename = Settings.GetRunningDirectory() +
"block_plane_0.dae";
// entry 2
entryName = ZipEntry.CleanName(Path.GetFileName(filename));
// Removes drive from name and fixes slash direction
newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
positionindex = 0;
modelist.Clear();
flightdata.Clear();
position = new List <Core.Geometry.Point3D> [200];
cmd.Clear();
cmdraw.Clear();
}
private void writeKML(string filename)
{
try
{
writeGPX(filename);
}
catch { }
Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink };
AltitudeMode altmode = AltitudeMode.absolute;
// all new logs have both agl and asl, we are using asl. this may break old logs
// if (MainV2.comPort.MAV.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
// altmode = AltitudeMode.relativeToGround; // because of sonar, this is both right and wrong. right for sonar, wrong in terms of gps as the land slopes off.
}
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
Style style = new Style();
style.Id = "yellowLineGreenPoly";
style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4));
PolyStyle pstyle = new PolyStyle();
pstyle.Color = HexStringToColor("7f00ff00");
style.Add(pstyle);
kml.Document.AddStyle(style);
int stylecode = 0xff;
int g = -1;
foreach (List <Point3D> poslist in position)
{
g++;
if (poslist == null)
{
continue;
}
LineString ls = new LineString();
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
Coordinates coords = new Coordinates();
coords.AddRange(poslist);
ls.coordinates = coords;
Placemark pm = new Placemark();
string mode = "";
if (g < modelist.Count)
{
mode = modelist[g];
}
pm.name = g + " Flight Path " + mode;
pm.styleUrl = "#yellowLineGreenPoly";
pm.LineString = ls;
stylecode = colours[g % (colours.Length - 1)].ToArgb();
Style style2 = new Style();
Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff, (stylecode >> 0) & 0xff);
log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString());
style2.Add(new LineStyle(color, 4));
pm.AddStyle(style2);
fldr.Add(pm);
}
Folder planes = new Folder();
planes.name = "Planes";
fldr.Add(planes);
Folder waypoints = new Folder();
waypoints.name = "Waypoints";
fldr.Add(waypoints);
LineString lswp = new LineString();
lswp.AltitudeMode = AltitudeMode.relativeToGround;
lswp.Extrude = true;
Coordinates coordswp = new Coordinates();
foreach (PointLatLngAlt p1 in cmd)
{
coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt));
}
lswp.coordinates = coordswp;
Placemark pmwp = new Placemark();
pmwp.name = "Waypoints";
//pm.styleUrl = "#yellowLineGreenPoly";
pmwp.LineString = lswp;
waypoints.Add(pmwp);
int a = 0;
int l = -1;
Model lastmodel = null;
foreach (Data mod in flightdata)
{
l++;
if (mod.model.Location.latitude == 0)
{
continue;
}
if (lastmodel != null)
{
if (lastmodel.Location.Equals(mod.model.Location))
{
continue;
}
}
Placemark pmplane = new Placemark();
pmplane.name = "Plane " + a;
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
try
{
pmplane.description = @"<![CDATA[
<table>
<tr><td>Roll: " + model.Orientation.roll + @" </td></tr>
<tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr>
<tr><td>Yaw: " + model.Orientation.heading + @" </td></tr>
<tr><td>WP dist " + mod.ntun[2] + @" </td></tr>
<tr><td>tar bear " + mod.ntun[3] + @" </td></tr>
<tr><td>nav bear " + mod.ntun[4] + @" </td></tr>
<tr><td>alt error " + mod.ntun[5] + @" </td></tr>
</table>
]]>";
}
catch { }
try
{
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude);
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";
model.Link = link;
pmplane.Model = model;
planes.Add(pmplane);
}
catch { } // bad lat long value
lastmodel = mod.model;
a++;
}
kml.Document.Add(fldr);
kml.Save(filename);
// create kmz - aka zip file
FileStream fs = File.Open(filename.Replace(".log.kml", ".kmz"), FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
// entry 1
string entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
File.Delete(filename);
filename = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "block_plane_0.dae";
// entry 2
entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction
newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(filename))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
positionindex = 0;
modelist.Clear();
flightdata.Clear();
position = new List <Core.Geometry.Point3D> [200];
cmd.Clear();
}
private static void Main(string[] args)
{
var logRepository = LogManager.GetRepository(Assembly.GetEntryAssembly());
XmlConfigurator.Configure(logRepository, new FileInfo("log4net.config"));
CommandLineParser.CommandLineParser parser =
new CommandLineParser.CommandLineParser();
ValueArgument <string> pathArg = new ValueArgument <string>(
'p', "path", "Path to a folder containing gravity vector files");
ValueArgument <string> outputArg = new ValueArgument <string>(
'o', "output", "Path to a folder for output KML files");
parser.Arguments.Add(pathArg);
parser.Arguments.Add(outputArg);
parser.ParseCommandLine(args);
if (pathArg.Parsed)
{
var path = Path.GetFullPath(pathArg.Value);
var outputPath = Path.GetFullPath(outputArg.Value);
if (Directory.Exists(path))
{
var files = Directory.GetFiles(path, "*", SearchOption.AllDirectories);
int fileCount = files.Count();
Log.Debug($"Loading {fileCount} gravity vectors");
Log.Debug("Starting..");
int processedFiles = 0;
ulong accRecords = 0;
Parallel.ForEach(files, file =>
{
try
{
string kmlFileName = Path.Combine(outputPath, Path.GetFileName(file) + ".kml");
Serializer serializer = new Serializer();
Kml kml = new Kml();
var currentClusterIndex = -1d;
DataModel.GravityVector previousGravityVector = null;
LineString previousClusterSegment = null;
Folder rootFolder = new Folder();
Folder folder = null;
var records = Util.ReadCsvFile <DataModel.GravityVector, GravityVectorCsvClassMap>(file);
accRecords += (ulong)(records.Count);
//Log.Debug($"Processing {Path.GetFileName(kmlFileName)}: {records.Count()} records");
foreach (var record in records)
{
// Skip the first record
if (previousGravityVector == null)
{
previousGravityVector = record;
continue;
}
LineString clusterSegment = new LineString();
clusterSegment.AltitudeMode = AltitudeMode.ClampToGround;
clusterSegment.Coordinates = new CoordinateCollection();
bool newCluster = false;
if (currentClusterIndex != record.ClusterIndex)
{
currentClusterIndex = record.ClusterIndex;
folder = new Folder();
folder.Name = "Cluster " + currentClusterIndex.ToString();
rootFolder.AddFeature(folder);
newCluster = true;
}
if (!newCluster)
{
clusterSegment.Coordinates.Add(new Vector(previousGravityVector.Latitude, previousGravityVector.Longitude));
}
clusterSegment.Coordinates.Add(new Vector(record.Latitude, record.Longitude));
Placemark placemark = new Placemark();
placemark.AddStyle(new Style()
{
Line = new LineStyle()
{
PhysicalWidth = record.DistanceMedian,
Color = GetColorFromSpeed(record.SpeedOverGround),
}
});
placemark.Geometry = clusterSegment;
placemark.Name = $"Grid: {record.GridId} / Distance: {record.DistanceMedian} / Speed: {record.SpeedOverGround} / Course: {record.CourseOverGround}";
folder.AddFeature(placemark);
// Do this last!
previousGravityVector = record;
previousClusterSegment = clusterSegment;
}
kml.Feature = rootFolder;
serializer.Serialize(kml);
File.WriteAllText(kmlFileName, serializer.Xml);
processedFiles++;
}
catch (Exception e)
{
Log.Error(e);
}
if (processedFiles % 5 == 0)
{
Log.Debug($"Processed {processedFiles}/{fileCount} files ({((double)processedFiles / (double)fileCount).ToString("0.00%")}) for a total of {accRecords} records");
}
});
}
else
{
Log.Debug("Path not found: " + path);
}
}
else
{
Log.Debug("You must specify --path");
}
Log.Debug("Done!");
}