public GoogleMaps(String _datasetFolder, String _polygonName)
{
datasetFolder = _datasetFolder;
polygonName = _polygonName;
utm = new UTM2Geodetic(); //added jek 12/27/2013
// JEKain 11/6/2013
// The Google Static Maps API has the following usage limits:
// 25 000 free static map requests per application per day
//initialize the current time to one hour ago
//used in measuring the elapsed time since a map request.
//cant find any limits on maps per hour -- jekain 11/6/2103
timeSinceLastMapRequest = DateTime.Now.AddHours(-1);
MAPAPIREQUESTLIMIT = 300;
}
public COVERAGE_TYPE polygonGeometry()
{
///////////////////////////////////////////////////////////////////////
// get the mean polygon location and the polygon area
// convert all vertices to UTM
// get the UTM zone that will be used
// get recangular bounding box
// determine the orientation of the point-scatter covariance ellipse
///////////////////////////////////////////////////////////////////////
utm = new UTM2Geodetic(); //class for LL to UTM conversion
for (int i = 0; i < polygons.Count; i++)
{
//local copy of the polygon -- re-inserted into the polygon list at the end of this procedure
Polygon p = polygons[i];
int numLatLonPoints = p.latitudeDeg.Count;
//mean Northing and Easting from mean lat/lon -- also use this to get the UTMZone that will be held fixed for this polygon
p.meanNorthing = 0.0;
p.meanEasting = 0.0;
//NOTE: get the UTMZone here (for the polygon mean location) that will be used for the whole project
utm.LLtoUTM(p.meanLatDeg * Deg2Rad, p.meanLonDeg * Deg2Rad, ref p.meanNorthing, ref p.meanEasting, ref p.UTMZone, false);
//the UTM zone will be held constant across the polygon points -- deals with polys that cross UTM zones
//get the Northing and Easting from the lat/lon poly points
double northing = 0.0, easting = 0.0;
p.maxNorthing = -9999999.0; p.maxEasting = -999999999.0; p.minNorthing = 999999999.0; p.minEasting = 99999999999.0;
p.maxLatDeg = -99999.0; p.maxLonDeg = -9999999.0; p.minLatDeg = 99999.0; p.minLonDeg = 999999.0;
double CovXX = 0.0, CovYY = 0.0, CovXY = 0.0; //covariance of the UTM poly points
for (int ipnts = 0; ipnts < numLatLonPoints; ipnts++)
{
utm.LLtoUTM(p.latitudeDeg[ipnts] * Deg2Rad, p.longitudeDeg[ipnts] * Deg2Rad, ref northing, ref easting, ref p.UTMZone, true);
p.Northing.Add(northing); p.Easting.Add(easting);
if (northing > p.maxNorthing) p.maxNorthing = northing;
if (northing < p.minNorthing) p.minNorthing = northing;
if (easting > p.maxEasting) p.maxEasting = easting;
if (easting < p.minEasting) p.minEasting = easting;
if (p.latitudeDeg[ipnts] > p.maxLatDeg) p.maxLatDeg = p.latitudeDeg[ipnts];
if (p.latitudeDeg[ipnts] < p.minLatDeg) p.minLatDeg = p.latitudeDeg[ipnts];
if (p.longitudeDeg[ipnts] > p.maxLonDeg) p.maxLonDeg = p.longitudeDeg[ipnts];
if (p.longitudeDeg[ipnts] < p.minLonDeg) p.minLonDeg = p.longitudeDeg[ipnts];
//covariance elements used to compute the principal axes of the polygon
//here we are assuming a north (X), east (Y), down (Z) corrdinatate system
CovYY += (easting - p.meanEasting) * (easting - p.meanEasting);
CovXX += (northing - p.meanNorthing) * (northing - p.meanNorthing);
CovXY += (easting - p.meanEasting) * (northing - p.meanNorthing);
}
CovYY /= 1000*numLatLonPoints;
CovXX /= 1000*numLatLonPoints;
CovXY /= 1000*numLatLonPoints;
//Compute the orientation of the principal axis of the covariance ellipse
//this is used to determine the best flying direction
//eigenvalues of covariance matrix -- see below site for equations for eigenvalues/vectors for 2D matrix
// people.csail.mit.edu/bkph/articles/Eigenvectors.pdf
double det = Math.Sqrt( (CovXX - CovYY)*(CovXX - CovYY) + 4.0 * CovXY * CovXY );
double lambda1 = ( CovXX + CovYY + det )/ 2.0;
double lambda2 = ( CovXX + CovYY - det )/ 2.0;
//the larger of these is the length of the major axis
//smaller is length of the minor axis
//the angle associated with the larger eigenvalue is the angle of the major axis
//eigenvectors should be 90 deg apart
if (lambda1 > lambda2)
{
//eigenvectors of covariance matrix
double eigen1X = 2.0 * CovXY;
double eigen1Y = CovYY - CovXX + det;
double ang1 = Math.Atan(eigen1Y / eigen1X) * Constants.Rad2Deg;
p.rotationAngleDeg = ang1;
}
else
{
//eigenvectors of covariance matrix
double eigen2X = 2.0 * CovXY;
double eigen2Y = CovYY - CovXX - det;
double ang2 = Math.Atan(eigen2Y / eigen2X) * Constants.Rad2Deg;
p.rotationAngleDeg = ang2;
}
//reflect a negative major axis direction by 180 deg (major axis can go either direction)
if (p.rotationAngleDeg < 0) p.rotationAngleDeg += 180.0;
//this must be done after creating the UTM Northing & Easting from the geodetic coordinates
polygonMath polyMath = new polygonMath(p.Easting, p.Northing, datasetFolder);
//compute rectangle bounds using the principal axes
double maxAlongPA = -9999999999, minAlongPA = 9999999999, distanceA = 0.0;
double maxOrthoPA = -9999999999, minOrthoPA = 9999999999, distanceO = 0.0;
for (int ipnts = 0; ipnts < numLatLonPoints; ipnts++)
{
//note below that the X components is Northing and the Y components are Easting
//The "line" is defined by its origin and rotation angle (positive east of north)
//the "point" is the ipnts vertex of the polygon
distanceO = polyMath.distanceFromPoint2Line( //distance orthogonal to the pricipal axis
new PointD(p.meanNorthing, p.meanEasting),
p.rotationAngleDeg,
new PointD(p.Northing[ipnts], p.Easting[ipnts]));
distanceA = polyMath.distanceFromPoint2Line( //distance orthogonal to the pricipal axis
new PointD(p.meanNorthing, p.meanEasting),
p.rotationAngleDeg + 90,
new PointD(p.Northing[ipnts], p.Easting[ipnts]));
if (distanceA > maxAlongPA) maxAlongPA = distanceA;
if (distanceA < minAlongPA) minAlongPA = distanceA;
if (distanceO > maxOrthoPA) maxOrthoPA = distanceO;
if (distanceO < minOrthoPA) minOrthoPA = distanceO;
}
//length and width of a bounding box with long-side along principal axis
//length should always be larger than the width
double lengthKm = (maxAlongPA - minAlongPA) / 1000.0;
double widthKm = (maxOrthoPA - minOrthoPA) / 1000.0;
//Constants.Log("Principal Axis bounding box: " + lengthKm.ToString("F1") + " X " + widthKm.ToString("F1"));
//if (lengthKm < widthKm)
// MessageBox.Show("Principal axes of polygon not longer than minor axis");
//principal axis direction is defined by p.rotationAngleDeg
//set the corner of the principal axes bounding box to be negative-going along the major and minor axis
//this corner will also become the origion of the flight line reference coordinate system
//this will correspond to a south-east corner for a north-going princpal axis
p.boundingBoxCornerNorthing =
Math.Cos(p.rotationAngleDeg * Constants.Deg2Rad) * (p.meanNorthing + minAlongPA) +
Math.Sin(p.rotationAngleDeg * Constants.Deg2Rad) * (p.meanEasting + minOrthoPA);
p.boundingBoxCornerEasting =
-Math.Sin(p.rotationAngleDeg * Constants.Deg2Rad) * (p.meanNorthing + minAlongPA) +
Math.Cos(p.rotationAngleDeg * Constants.Deg2Rad) * (p.meanEasting + minOrthoPA);
//TODO: should just return the polygon points and fill the p structure after the return
p.areasqkm = polyMath.areaOfPolygon();
p.areasqmi = p.areasqkm * 1000000.0 / ((0.3048 * 5280.0) * (0.3048 * 5280.0));
Constants.Log("");
if (p.polyCoverageType == COVERAGE_TYPE.polygon)
{
Constants.Log("Polygon area: " + p.areasqmi.ToString("F3") + " (sqmi) " + p.areasqkm.ToString("F3") + " (sqkm)");
}
else if (p.polyCoverageType == COVERAGE_TYPE.linearFeature)
{
double pathRange = polyMath.pathLength();
Constants.Log("Path length: " + (pathRange/0.3048/5280.0).ToString("F2") +
" (mi) " + (pathRange/1000.0).ToString("F2") + " (km)");
}
Constants.Log("");
///////////////////////////////////////////////////////////////////////////////////////////////
//write the summary information for this polygon
//outfile.WriteLine();
outfile.WriteLine("PolygonName: {0} PolyPoints= {1,4} Area= {2,9:####.00} (kmsq) {3,9:####.00} (sqmi)",
p.PolygonName, numLatLonPoints.ToString(), p.areasqkm, p.areasqmi);
//foreach (airport apt in apts)
// outfile.WriteLine(" {0,50} distance (mi) {1,8:####.00}", apt.name, apt.distanceToPolyCenterMi);
/////////////////////////////////////////////////////////////////////////////////////////////////
//we have modified a copy of the polygon object -- re-insert it into the polygon list
polygons[i] = p;
}
//NOTE: here we are assuming that all the coverage types are identical
//generally we only use a single coverage for a mission plan session
return polygons[0].polyCoverageType;
}
public void createBackgroundMapsForLinearFeature(List<LINEAR_FEATURE> linearFeatures, String UTMZone, bool downloadMap)
{
/////////////////TODO -- put in a input file
////////////////////////////////////////////////////////////////////////////
double maxIncrementAlonPathMeters = 5.0 * 5280.0 * 0.3048; // 5.0 miles
////////////////////////////////////////////////////////////////////////////
for (int iFeat = 0; iFeat < linearFeatures.Count; iFeat++)
{
LINEAR_FEATURE linearFeature = linearFeatures[iFeat];
polygonMath polyMath = new polygonMath(linearFeature.EastingProNavS, linearFeature.NorthingProNavS, datasetFolder);
UTM2Geodetic utm = new UTM2Geodetic();
linearFeature.NWMapCorner = new List<PointD>();
linearFeature.SEMapCorner = new List<PointD>();
linearFeature.mapNames = new List<string>();
//find a deltaRange along the path < maxIncrementAlonPathMeters such that deltaRange * N = totalPathLength
int nMaps = (int)(linearFeature.pathLength / maxIncrementAlonPathMeters);
//above gives a number of increments such that one additional increment will fall outside the endpoint.
double incrementAlongPath = linearFeature.pathLength / (nMaps + 1);
//this gives an incerment that is < maxIncrementAlonPathMeters such that
//there will be nMaps+1 increments that end exactly on the end of the path
//We will need nMaps + 2 maps with one map at the beginning and one map at the end
int totalMaps = nMaps + 2;
//this lets us put N equispaced 10mix10mi maps along the path.
for (int i = 0; i < totalMaps; i++)
{
/////////////////////
//get a map
/////////////////////
//build up the URL to retrieve the Google Map -- see the above model for the syntax
//this zoom level gives ~ 10mi X 10mi
int zoom = 12;
//increment the path
PointD pap = polyMath.pointAlongPathFromStart(i * incrementAlongPath);
double latitude = 0.0, longitude = 0.0;
utm.UTMtoLL(pap.Y, pap.X, UTMZone, ref latitude, ref longitude);
setMap(zoom, new PointD(longitude, latitude));
/* below used only to get the width & height of the returned map with zoom = 12
double NWNorthing = 0.0, NWEasting = 0.0;
double SENorthing = 0.0, SEEasting = 0.0;
utm.LLtoUTM(mapNWCornerCordinates.Y * Constants.Deg2Rad, mapNWCornerCordinates.X * Constants.Deg2Rad,
ref NWNorthing, ref NWEasting, ref UTMZone, true);
utm.LLtoUTM(mapSECornerCordinates.Y * Constants.Deg2Rad, mapSECornerCordinates.X * Constants.Deg2Rad,
ref SENorthing, ref SEEasting, ref UTMZone, true);
double mapNSDimensionMi = (NWNorthing - SENorthing) / 0.3048 / 5280.0;
double mapEWDimensionMi = (SEEasting - NWEasting) / 0.3048 / 5280.0;
* */
linearFeature.NWMapCorner.Add(mapNWCornerCordinates);
linearFeature.SEMapCorner.Add(mapSECornerCordinates);
String GoogleMapsURL = "http://maps.googleapis.com/maps/api/staticmap?center=" +
latitude.ToString() + "," +
longitude.ToString() + "&zoom=" + zoom.ToString() + "&size=640x480&sensor=false";
//this downloads a map that can be opened --- need to figure out the scaling...
//GoogleMapsURL = "http://dev.virtualearth.net/REST/v1/Imagery/Map/Road/Routes?wp.0=Seattle,WA;64;1&wp.1=Redmond,WA;66;2&key=" +
// Constants.bingKey;
//set the file storage location
String backgroundImageFolder = datasetFolder + "\\" + polygonName + "_Background";
if (!Directory.Exists(backgroundImageFolder)) Directory.CreateDirectory(backgroundImageFolder);
String SaveToFile = backgroundImageFolder + "\\linearMap_" + iFeat.ToString("D2") +"_" + i.ToString("D3") + ".png";
if (downloadMap)
{
//get the file from the web using a webClient
getFileFromWeb(GoogleMapsURL, SaveToFile);
}
linearFeature.mapNames.Add(SaveToFile);
}
}
}
public ReflyPlanner(String _reflyKmlPathFilename, Polygon _p)
{
reflyKmlPathFilename = _reflyKmlPathFilename;
p = _p;
UTM2Geodetic utm = new UTM2Geodetic();
XmlTextReader textReader = new XmlTextReader(reflyKmlPathFilename); //associate the textReader with input file
while (textReader.Read())
{
textReader.MoveToElement();
if (textReader.IsStartElement() && textReader.Name == "coordinates")
{
/////////////////////////////////////////////////////////////////////
//read in and process the coordinates into refly line segments
/////////////////////////////////////////////////////////////////////
textReader.Read(); //here we read the complete set of coordinates as a single string
//textReader.Value contains a string with all the polygon coordinates
char[] delimiterChars = { ',', ' ', '\t', '\n', '\r' }; //these delimiters were determined by looking at a file ...
//the "value" is the text between the <coordinates> and </coordinates>
//below we read the complete string value and split it into separate substrings -- ugly but it works
//the substrings contain the individual coordinate values with some ""s and the heigts are "0".
string[] coordinateValues = textReader.Value.ToString().Split(delimiterChars);
//get the quantitative lat/long values from the text array ... there are a number of ""s in the text file ...
//each Google point has three values : longitude, Latitude, height -- we assume the height is zero here
int k = 0; int i = 0;
double lat1=0.0, lat2=0.0, lon1=0.0, lon2=0.0;
while (i < coordinateValues.Count())
{
if (coordinateValues[i] != "")
{
lat1 = Convert.ToDouble(coordinateValues[i + 1]);
lon1 = Convert.ToDouble(coordinateValues[i]);
//odd-even numbered (k) points are the endpoints
if ( (k+1) % 2 == 0)
{
//have found a segment pair of lat/lon points
//order the start/end points so the start end is to the south
endPoints ep = new endPoints();
if (lat1 > lat2)
{
ep.endLat = lat1;
ep.endLon = lon1;
ep.startLat = lat2;
ep.startLon = lon2;
}
else
{
ep.endLat = lat2;
ep.endLon = lon2;
ep.startLat = lat1;
ep.startLon = lon1;
}
utm.LLtoUTM(ep.startLat * Deg2Rad, ep.startLon * Deg2Rad, ref ep.startUTMY, ref ep.startUTMX, ref p.UTMZone, true);
utm.LLtoUTM(ep.endLat * Deg2Rad, ep.endLon * Deg2Rad, ref ep.endUTMY, ref ep.endUTMX, ref p.UTMZone, true);
reflySegments.Add(ep);
}
lat2 = lat1;
lon2 = lon1;
k++; //index of the storage array
//increment the split array by 3 because the points are lat,lon,height
i += 3; //increment by 3 to get the next coordinate
}
else i++; //here we skip the ""s in the text array
}
}
}
}
//this procedure is not called ............... jekain 11/10/2013 .. we use the procedure below this
public terrainStats getTerrainStatsInPoly( List<PointD> missionPolygon)
{
//////////////////////////////////////////////////////////////////////////////////////
//use the Google elevation API to get the average terrain height inside a polygon
//////////////////////////////////////////////////////////////////////////////////////
terrainStats ts;
List<double> heights = new List<double>();
ts.avgTerrainHeight = 0.0;
ts.sigTerrainHeight = 0.0;
ts.maxTerrainHeight = -999999999.0;
ts.minTerrainHeight = 999999999.0;
ts.terrain90percentileHeight = 0.0;
int numPointsInXml = 0;
UTM2Geodetic utm = new UTM2Geodetic();
List<double> latA = new List<double>();
List<double> lonA = new List<double>();
////////////////////////////////////////////////
int maxSamplesPerRequest = 200;
int numberOfRequests = 1;
////////////////////////////////////////////////
int maxNumberOfSamples = maxSamplesPerRequest * numberOfRequests; //not used ...
double maxLat = -999999999.0;
double minLat = 999999999.0;
double maxLon = -999999999.0;
double minLon = 999999999.0;
//get the polygon bounds where we will draw heights to determine the average
for (int i = 0; i < missionPolygon.Count; i++)
{
if (missionPolygon[i].X > maxLon) maxLon = missionPolygon[i].X;
if (missionPolygon[i].X < minLon) minLon = missionPolygon[i].X;
if (missionPolygon[i].Y > maxLat) maxLat = missionPolygon[i].Y;
if (missionPolygon[i].Y < minLat) minLat = missionPolygon[i].Y;
latA.Add(missionPolygon[i].Y); //ugly -- cause the inPoly test below takes individual point pairs
lonA.Add(missionPolygon[i].X);
}
//get average terrain height based on numberOfPoints points placed around the polygon
//place points randomly around the bounding polygon
//example for two elevations: http://maps.googleapis.com/maps/api/elevation/xml?locations=39.7391536,-104.9847034|39.7,-104.9&sensor=false
// using a polyline encoding ... locations=enc:gfo}EtohhU
//prefix and postfix to be used in creating the eleavation server URL command
String elevationAPIPrefix = "http://maps.googleapis.com/maps/api/elevation/xml?locations=enc:";
String elevationAPIPostfix = "&sensor=false";
//////////////////////////////////////////////////////////////////////////////////////////////////////
//get RnumInside lat/lon random-spaced points inside the polygon
//string together a lot of elevation requests at once to reduce calls to the Google elevation server
//////////////////////////////////////////////////////////////////////////////////////////////////////
for (int it = 0; it < numberOfRequests; it++)
{
int numInside = 0, ranC = 0; String ElevationAPIRequest = elevationAPIPrefix;
Random rand = new Random();
int RnumInside = maxSamplesPerRequest; // 100 appears to be near the limit -- if larger the url gets too long for the request
List<PointD> pointsInside = new List<PointD>();
while (numInside < RnumInside && ranC < 10000)
{
ranC++;
//get a random location inside the polygon bounding box
double xLoc = minLon + (maxLon - minLon) * rand.NextDouble();
double yLoc = minLat + (maxLat - minLat) * rand.NextDouble();
//check to see if the random point is actually inside the polygon
if (pointInsidePolygon(missionPolygon.Count, xLoc, yLoc, lonA, latA))
{
numInside++;
//if the randonly generated point is inside the polygon, add a request for the terrain height at the point
// Form the Google Eevation API request
// 1/60 deg = 6000 ft 0.001 deg = 6000 * 60 / 1000 ft = 360 ft -> "F3" gives the nearest 360 ft
//need to keep the total URL below 2048 characters
ElevationAPIRequest += yLoc.ToString("F3") + "," + xLoc.ToString("F3"); //need to clip the digits to keep the url short ...
pointsInside.Add(new PointD(xLoc, yLoc));
//pointsInside.Add(new PointD(144.123 + (double)numInside / 1000.0, -35.123 + (double)numInside / 1000.0));
if (numInside < RnumInside) ElevationAPIRequest += "|";
}
}
//if we get here, we have a elevation API request that includes maxSamplesPerRequest elevation requests
//the below commented calls allow you to look at the downloaded xml data
WebClient ww = new WebClient();
//the belw command uses the polylineEncoder (Google procedure) to reduce the size of the URL string
//this allows more elevations to be commanded in a single call to the elevation API
String encodedPoints = polylineEncoder.EncodeCoordinates(pointsInside);
//now form the conmplete URL command string
String ss = elevationAPIPrefix + encodedPoints + elevationAPIPostfix;
//compare the effects of the polyline encoding
int charsInEncodedString = ss.Length;
int charsInNonencodedString = ElevationAPIRequest.Length;
//use the polyline encoded string
Uri uri = new Uri(ss);
//Prevent the elevation API data requests from occurring faster than a set rate.
int elapsedTimeMillisecs = (DateTime.Now - lastElevationAPIAccess).Milliseconds;
if (elapsedTimeMillisecs < ELEVATIONAPIREQUESTLIMIT)
{
Thread.Sleep(ELEVATIONAPIREQUESTLIMIT - elapsedTimeMillisecs);
}
//this waits til the download is complete ...
String downloadedData = ww.DownloadString(uri); //download into a String -- blocking call
elevationQueries++;
lastElevationAPIAccess = DateTime.Now;
//String saveToFile = datasetFolder + @"\temp.xml";
//if (File.Exists(saveToFile)) File.Delete(saveToFile);
//ww.DownloadFile(uri, saveToFile); //down directly to a file perform the web download -- blocking call
//StreamReader sr = new StreamReader(saveToFile);
// XmlTextReader tr = new XmlTextReader(sr);
//the "downloadedData" string was downloaded from the URL command response and contaoins the elevation results
XmlReader tr = XmlReader.Create(new StringReader(downloadedData)); //create a xmlReader from a String -- obtained from the Google Elevation API
//read off the elevations from the xml response to the terrain height request to the Google elevation API
while (tr.Read()) //read the xml reply from the elevation API to get the elevation
{
if (tr.IsStartElement() && tr.Name == "elevation")
{
tr.Read();
double altitude = Convert.ToDouble(tr.Value);
numPointsInXml++;
ts.avgTerrainHeight += altitude;
ts.sigTerrainHeight += altitude * altitude;
if (altitude > ts.maxTerrainHeight) ts.maxTerrainHeight = altitude;
if (altitude < ts.minTerrainHeight) ts.minTerrainHeight = altitude;
heights.Add(altitude); //just accumulate the heights cause we just want the average
}
else if (tr.IsStartElement() && tr.Name == "status")
{
tr.Read();
if (tr.Value == "OVER_QUERY_LIMIT") //dont recall ever seeing this response -- jekain 11/10/2013 ...
{
ELEVATIONAPIREQUESTLIMIT *= 2;
System.Windows.Forms.MessageBox.Show(" Google Elevation API Over Query Limit. Queries this run: " +
elevationQueries.ToString() + "new msec-between-request " + ELEVATIONAPIREQUESTLIMIT.ToString() );
tr.Close();
//sr.Close();
return ts;
}
}
}
tr.Close();
//sr.Close();
}//end if for loop over "it"
//sort all the terrain heights in oreder to ascertain the 90 percentile height ...
heights.Sort(); int pc90 = (int)(0.90 * numPointsInXml) - 1; //for 50 samples -- this gives element heights[44]
ts.terrain90percentileHeight = heights[pc90];
ts.avgTerrainHeight /= numPointsInXml;
ts.sigTerrainHeight = Math.Sqrt(ts.sigTerrainHeight/numPointsInXml - ts.avgTerrainHeight * ts.avgTerrainHeight);
return ts;
}
public void generateSmoothTrajectoryWithProNav(String DataSetFolder, String UTMZone, bool computeProjections, ref double maxBankDeg,
double ProNavGain, double deltaTime, double DistanceToTarget, double vehicleVelocity, double altitude, double swathWidth,
List<double> NorthingIn, List<double> EastingIn, List<double> NorthingOut, List<double> EastingOut,
List<double> NorthingProjected, List<double> EastingProjected,
List<double> NorthingIP1, List<double> EastingIP1, List<double> NorthingIP2, List<double> EastingIP2)
{
//////////////////////////////////////////////////////////////////////////////////////////
//using an input set of path points generated by Google earth, create a smoothed
//trajectory flyable by an aircraft that passes near the points.
//////////////////////////////////////////////////////////////////////////////////////////
//initialize the state at first vertex with heading along first segment
double X = NorthingIn[0];
double Y = EastingIn[0];
//heading from 0th point towards the 1th point -- heading is positive CCW from north
double heading = Math.Atan2(EastingIn[1] - EastingIn[0],
NorthingIn[1] - NorthingIn[0]);
//add the initial position to the output trajectory
NorthingOut.Add(X);
EastingOut.Add(Y);
//stop a segment when the ProNav acceleration command results in a bank greater than some max bank threshold
bool stopThisSegment = false;
polygonMath polyMath = new polygonMath(EastingIn, NorthingIn, datasetFolder);
UTM2Geodetic utm = new UTM2Geodetic();
int lastXYVertex = 0;
double distanceToNextVertex = 0;
int iTime = 0;
double bankCommand = 0.0;
double maxBank = -999999.0;
//integrate the state in the while loop
while (!stopThisSegment)
{
//////////////////////////////
//state: X Y heading
//////////////////////////////
//get a point on the path that is orthognal to the velocity vector
PointD pca = polyMath.pointOnPathOrthogonalToVelocityVector(heading, new PointD(X, Y),
ref lastXYVertex, ref distanceToNextVertex,
EastingIn, NorthingIn);
//if (lastXYVertex == 1) break;
//stopping condition -- have gone beyond last vertex in the path
if (-distanceToNextVertex > 0)
{
Console.WriteLine(" stopping on distanceToNextVertex " + distanceToNextVertex.ToString());
stopThisSegment = true;
}
//locate a point ahead of (X,Y) along the path by a prescribed distance
//this becomes the target for pro nav guidance
//placing the target farther ahead reduces the max bank but smooths the points
PointD pAhead = polyMath.FuturePointOnLineFeature(
pca, lastXYVertex, distanceToNextVertex, DistanceToTarget,
EastingIn, NorthingIn);
//X is north and Y is east
double velX = vehicleVelocity * Math.Cos(heading);
double velY = vehicleVelocity * Math.Sin(heading);
//compute the Line-Of-Sight rate between the platform and the target point
//the heading_dot will be commanded to -3*LOSRate per the ProNav
// X -- North Y -- East -- causes heading to be positive CCW rotation for Right Hand coordinate system
double delX = pAhead.X - X;
double delY = pAhead.Y - Y;
double LOSAngle = Math.Atan2(delY, delX);
double LOSRate = Math.Cos(LOSAngle) * Math.Cos(LOSAngle) * (-velY / delX + delY * velX / (delX * delX));
X += velX * deltaTime;
Y += velY * deltaTime;
////////////////////////////////////////////////////
//generate the output file for the trajectory
////////////////////////////////////////////////////
NorthingOut.Add(X);
EastingOut.Add(Y);
double accelCommand = ProNavGain * LOSRate * vehicleVelocity; //units: m/s/s
//pursuit navigation
//double accelCommand = -0.10 * (heading - LOSAngle) * vehicleVelocity;
//bank command necessary to maintain level flight and achieve command lateral acceleration
bankCommand = Math.Atan(accelCommand / Constants.Gravity); // degrees
if (Math.Abs(bankCommand) > maxBank) maxBank = Math.Abs(bankCommand);
heading += (accelCommand / vehicleVelocity) * deltaTime;
if (accelCommand / Constants.Gravity > 100.0)
{
stopThisSegment = true;
Console.WriteLine(" stopping on the bank command threshold ");
}
if (iTime%3 == 0 && computeProjections)
{
double Xproj = X + altitude * Math.Tan(bankCommand) * velY / vehicleVelocity;
double Yproj = Y - altitude * Math.Tan(bankCommand) * velX / vehicleVelocity;
NorthingProjected.Add(Xproj);
EastingProjected.Add(Yproj);
//creat endpoints for an image taken at this location
NorthingIP1.Add(Xproj + swathWidth * velY / vehicleVelocity / 2.0 );
EastingIP1.Add( Yproj - swathWidth * velX / vehicleVelocity / 2.0 );
NorthingIP2.Add(Xproj - swathWidth * velY / vehicleVelocity / 2.0 );
EastingIP2.Add( Yproj + swathWidth * velX / vehicleVelocity / 2.0 );
//predicted miss distance
//if we assume the future point doesnt move, we can compute the miss distance
//that would occur if the continue with pronav until the point-of-closest-approach
double tgo = DistanceToTarget / vehicleVelocity;
double Xmiss = 3.0 * (X + velX * tgo) / tgo * tgo;
double Ymiss = 3.0 * (Y + velY * tgo) / tgo * tgo;
//miss vector orthogonal to flight direction
}
iTime++;
if (iTime > 5000)
{
stopThisSegment = true;
Console.WriteLine("stopping on count");
}
} //end of trajectory generator
maxBankDeg = maxBank*Constants.Rad2Deg;
Console.WriteLine(" Max bank command = " + maxBankDeg.ToString("F2") + " deg");
}
void LinearFeaturePlanner(Polygon p, double flightAlt, double swathWidthKm,
double aircraftSpeedKnots, double aircraftSpeedKPH, double DRImageHeightKm)
{
//if (p.polyCoverageType == COVERAGE_TYPE.linearFeature)
//{
//show the "PLAN" button
button3.Visible = true;
button3.Enabled = true;
///////////////////////////////////////////////////////////////////////////////////
// wait here til user clicks considers the coverage options and clicks: "PLAN"
///////////////////////////////////////////////////////////////////////////////////
while (!MissionPlanningInitiated)
{
Application.DoEvents(); //wait for user inputs
}
//TODO
//get the flyable aircraft paths that will cover the input linear feature path
//there will be one linearFeature object for each of the parallel paths
List<LINEAR_FEATURE> linearFeatures =
linearFeatureCoverage(numParallelPaths, linearPathCentering,
flightAlt, swathWidthKm * 1000.0, aircraftSpeedKnots * 0.514);
//the above procedure writes kml files for forward, backward, & smoothed trajectory
//also writes out the image boresight projection and the image endpoints as kml files
GoogleMaps ggl = new GoogleMaps(datasetFolder, p.PolygonName);
//get the project map for the path -- shows complete extent of the path and takeoff airport
ggl.getGoogleMapToCoverProject(ref p, downloadMap);
//get the path-segment maps that will be used to form a aircraft-centered sliding window map
//get rectangular maps sized 10mi x 10mi at 5mi intervals along the path
ggl.createBackgroundMapsForLinearFeature(linearFeatures, p.UTMZone, downloadMap);
double totalPathDistance = 0.0;
polygonMath polymath = new polygonMath(linearFeatures[0].EastingProNavS, linearFeatures[0].NorthingProNavS, datasetFolder);
//get the along-path altitudes
for (int iLF = 0; iLF < linearFeatures.Count; iLF++)
{
totalPathDistance += linearFeatures[iLF].pathLength;
List<PointD> pointsAlongPath = new List<PointD>();
UTM2Geodetic utm = new UTM2Geodetic();
//create a sequence of points along path based on the smoothed trajectory
for (int i = 0; i < linearFeatures[iLF].NorthingProNavS.Count; i++)
{
double lat = 0.0, lon = 0.0;
utm.UTMtoLL(linearFeatures[iLF].NorthingProNavS[i], linearFeatures[iLF].EastingProNavS[i], p.UTMZone, ref lat, ref lon);
pointsAlongPath.Add(new PointD(lon, lat));
}
//get the terrain heights using the Google Elevation API
linearFeatures[iLF].alongPathAltitude = polymath.getTerrainHeightsFromPointList(pointsAlongPath);
//number of triggered images along the path
linearFeatures[iLF].numImages = (int)(linearFeatures[iLF].pathLength / (DRImageHeightKm * 1000.0));
//get stats of the along-path altitudes
double maxAlt = -99999.0, minAlt = 99999.0, meanAlt = 0.0;
foreach (double alt in linearFeatures[iLF].alongPathAltitude)
{
if (alt > maxAlt) maxAlt = alt;
if (alt < minAlt) minAlt = alt;
meanAlt += alt;
}
meanAlt /= linearFeatures[iLF].alongPathAltitude.Count;
linearFeatures[iLF].maxAltitude = maxAlt;
linearFeatures[iLF].minAltitude = minAlt;
linearFeatures[iLF].meanAltitude = meanAlt;
linearFeatures[iLF].pathNumber = iLF;
}
//get the 2way ferry distance
double total2WayFerryDistance = 0.0;
//start: always start for 0th path to the selected airport
//distance from selected airport to start of first line
double delNS = linearFeatures[0].NorthingProNavS[0] - p.airports[selectedAirportIndex].northing;
double delES = linearFeatures[0].EastingProNavS[0] - p.airports[selectedAirportIndex].easting;
double distanceToAptS = Math.Sqrt(delNS * delNS + delES * delES);
total2WayFerryDistance += distanceToAptS;
//always return to the airport from the end of the last path
int endIndex = linearFeatures[linearFeatures.Count - 1].NorthingProNavS.Count - 1;
//distance from selected airport to start of first line
delNS = linearFeatures[linearFeatures.Count - 1].NorthingProNavS[endIndex] - p.airports[selectedAirportIndex].northing;
delES = linearFeatures[linearFeatures.Count - 1].EastingProNavS[endIndex] - p.airports[selectedAirportIndex].easting;
distanceToAptS = Math.Sqrt(delNS * delNS + delES * delES);
//final 2-way ferry distance is the sum of the to- and from- ferry distances
total2WayFerryDistance += distanceToAptS;
double totalPathTime = totalPathDistance / (aircraftSpeedKPH * 1000.0);
int totalLFImages = (int)(totalPathDistance / (DRImageHeightKm * 1000.0));
writeKmlLinearFeatureMissonPlan(p,
DRImageHeightKm, numParallelPaths, flightAlt, swathWidthKm,
totalPathDistance, total2WayFerryDistance, aircraftSpeedKPH,
totalPathTime, totalLFImages, linearFeatures);
//////////////////////////////////////////////
//show the END button
button2.Visible = true;
button2.Enabled = true;
//////////////////////////////////////////////
//wait here to see if the user clicks the "END" button
while (true)
{
Application.DoEvents();
}
//Environment.Exit(-1);
//}
//////////////////////// end of linear feature Planner //////////////////
}
