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;
}
List<LINEAR_FEATURE> linearFeatureCoverage(int numberParallelPaths, LINEAR_PATH_CENTERING centering,
double altAGLMeters, double swathWidth, double vehicleVelocityMperSec)
{
/////////////////////////////////////////////////////////////////////////////////////////////////////
//This procedure creates a mission plan from a kml linear path created by the Google Earth Path tool
//The geodetic path points (vertices) must have been generated using "InspectKMLforMissions()"
//In this procedure, the input path is "flown" using a 2D simulation and proportional navigation
//Capability is provided to allow multiple side-by-side parallel paths
//either centered over the inoput path of to the right of this path. Separate flight lines
//may be required for a single path if the path turns are too sharp.
/////////////////////////////////////////////////////////////////////////////////////////////////////
//TODO: get these from the input file
//pronav numerical integration parameters
double proNavGain = 3.0; //proportional Navigation Gain -- usually assumed to be 3.0
double deltaTime = 2.0; //integration step size along the trajectory for the pro nav simulation
double distanceToTarget = 2500.0; //diatance ahead along the path the "rabbit" is placed
double altitudeAGL = altAGLMeters;
double vehicleVelocity = vehicleVelocityMperSec;
List<LINEAR_FEATURE> linearFeatures = new List<LINEAR_FEATURE>();
//the following trajectories are constant for all parallel paths
//these represent the smoothed trajectory for the input path
//The parallel paths are parallel t the smoothed input path
List<double> EastingTrajectoryF = new List<double>();
List<double> NorthingTrajectoryF = new List<double>();
List<double> EastingTrajectoryB = new List<double>();
List<double> NorthingTrajectoryB = new List<double>();
List<double> EastingTrajectoryS = new List<double>();
List<double> NorthingTrajectoryS = new List<double>();
List<double> placeHolder = new List<double>();
//polyMat provides polygon and line path math tools
polygonMath polyMath = new polygonMath(polys[0].Easting, polys[0].Northing, datasetFolder);
String UTMZone = polys[0].UTMZone;
double maxBankDeg = 0.0;
//generate the pro nav smoothed forward trajectory
polyMath.generateSmoothTrajectoryWithProNav(datasetFolder, UTMZone, false, ref maxBankDeg,
proNavGain, deltaTime, distanceToTarget, vehicleVelocity, altitudeAGL, swathWidth,
polys[0].Northing, polys[0].Easting, NorthingTrajectoryF, EastingTrajectoryF,
placeHolder, placeHolder,
placeHolder, placeHolder, placeHolder, placeHolder);
//create a reversed set of path points so we can fly the path backwards
List<double> NorthingB = new List<double>();
List<double> EastingB = new List<double>();
for (int i = polys[0].Northing.Count - 1; i >= 0; i--)
{
NorthingB.Add(polys[0].Northing[i]);
EastingB.Add(polys[0].Easting[i]);
}
//generate the pro nav smoothed backward trajectory
polyMath.generateSmoothTrajectoryWithProNav(datasetFolder, UTMZone, false, ref maxBankDeg,
proNavGain, deltaTime, distanceToTarget, vehicleVelocity, altitudeAGL, swathWidth,
NorthingB, EastingB, NorthingTrajectoryB, EastingTrajectoryB,
placeHolder, placeHolder,
placeHolder, placeHolder, placeHolder, placeHolder);
List<double> NorthingOutBrev = new List<double>();
List<double> EastingOutBrev = new List<double>();
//reverse this trajectory so we can match it to the forward trajectory
for (int i = NorthingTrajectoryB.Count - 1; i >= 0; i--)
{
NorthingOutBrev.Add(NorthingTrajectoryB[i]);
EastingOutBrev.Add(EastingTrajectoryB[i]);
}
//
//average the two paths to get a smoothed path that can be flown both ways
int numRecs = NorthingTrajectoryF.Count;
double distanceToNextVertex = 0;
int index = 0;
if (NorthingTrajectoryF.Count > NorthingTrajectoryB.Count)
numRecs = NorthingTrajectoryB.Count;
//go through all points and do the averaging
for (int i = 0; i < numRecs; i++)
{
//increment through points in forward trajectory
//locate point-of-closest-approach on the backward trajectorty
//average these two points to get the smoothed trajectory
PointD pca = polyMath.distanceFromPoint2Linefeature(
new PointD(NorthingTrajectoryF[i], EastingTrajectoryF[i]),
ref index, ref distanceToNextVertex, NorthingOutBrev, EastingOutBrev);
NorthingTrajectoryS.Add((NorthingTrajectoryF[i] + pca.X) / 2.0);
EastingTrajectoryS.Add((EastingTrajectoryF[i] + pca.Y) / 2.0);
}
/*
//the following diagnostic kml files are constant for each path
prepareKmlForUTMPointList(datasetFolder, "TrajectoryF",
NorthingTrajectoryF, EastingTrajectoryF, UTMZone);
prepareKmlForUTMPointList(datasetFolder, "TrajectoryB",
NorthingTrajectoryB, EastingTrajectoryB, UTMZone);
prepareKmlForUTMPointList(datasetFolder, "TrajectoryS ",
NorthingTrajectoryS, EastingTrajectoryS, UTMZone);
* */
//prepare the trajectory information for the offset parallel paths
//NOTE: path 0 is always assumed to be the initial path flown from start-to-end as defined by the input data
// successive even paths are flown end-to-start (reversed direction)
// successive odd paths are flown start-to-end
for (int iPar = 0; iPar < numberParallelPaths; iPar++)
{
LINEAR_FEATURE linearFeature = new LINEAR_FEATURE();
linearFeature.proNavGain = proNavGain;
linearFeature.rabbitAheadDistance = distanceToTarget;
linearFeature.EastingProNavS = new List<double>();
linearFeature.NorthingProNavS = new List<double>();
linearFeature.EastingProjection = new List<double>();
linearFeature.NorthingProjection = new List<double>();
linearFeature.EastingImage1 = new List<double>();
linearFeature.NorthingImage1 = new List<double>();
linearFeature.EastingImage2 = new List<double>();
linearFeature.NorthingImage2 = new List<double>();
linearFeature.alongPathAltitude = new List<double>();
//this where we offset the input path depending on numberParallelPaths & LINEAR_PATH_CENTERING
//generate an offset trajectory to the smoothed trajectory
polygonMath polyMath2 = new polygonMath(EastingTrajectoryS, NorthingTrajectoryS, datasetFolder);
double offset = 0.0;
if ( centering == LINEAR_PATH_CENTERING.centeredOnInputPath) offset = -(numberParallelPaths - 1) * swathWidth / 2.0 + iPar * swathWidth;
else if (centering == LINEAR_PATH_CENTERING.offsetToLeftOfInputPath) offset = iPar * swathWidth;
else if (centering == LINEAR_PATH_CENTERING.offsetToRightOfInputPath) offset = -iPar * swathWidth;
offset *= 0.70; ///this accounts for a 30% sidelap between paths
//create the current path when multiple paths are requested
List<double> EastingParallel = new List<double>();
List<double> NorthingParallel = new List<double>();
//return a parallel path from the input smoothed path
polyMath2.parallelPath(ref EastingParallel, ref NorthingParallel, offset);
//should this be polyMath2 ???
linearFeature.pathLength = polyMath.pathLength();
//create a reversed set of path points so we can fly the path backwards
List<double> NorthingRev = new List<double>();
List<double> EastingRev = new List<double>();
if (iPar % 2 != 0) //locate the odd paths (1, 3, 5, 7 ...
{
for (int i = EastingTrajectoryS.Count - 1; i >= 0; i--)
{
NorthingRev.Add(NorthingParallel[i]);
EastingRev.Add(EastingParallel[i]);
}
for (int i = 0; i < EastingTrajectoryS.Count; i++)
{
NorthingParallel[i] = NorthingRev[i];
EastingParallel[i] = EastingRev[i];
}
}
//generate the pro nav solution for the smoothed forward/backward trajectory
//this is the trajectory that is expected from the actual flight mission
polyMath.generateSmoothTrajectoryWithProNav(datasetFolder, UTMZone, true, ref maxBankDeg,
proNavGain, deltaTime, distanceToTarget/2.0, vehicleVelocity, altitudeAGL, swathWidth,
NorthingParallel, EastingParallel,
linearFeature.NorthingProNavS, linearFeature.EastingProNavS,
linearFeature.NorthingProjection, linearFeature.EastingProjection,
linearFeature.NorthingImage1, linearFeature.EastingImage1,
linearFeature.NorthingImage2, linearFeature.EastingImage2);
String parallelPathNotation= "";
if (numberParallelPaths > 0) parallelPathNotation = iPar.ToString("D2");
/*
//the following kml output files vary with each parallel path
prepareKmlForUTMPointList(datasetFolder, "ParallelPath" + parallelPathNotation,
NorthingParallel, EastingParallel, UTMZone);
prepareKmlForUTMPointList(datasetFolder, "ProNavS" + parallelPathNotation,
linearFeature.NorthingProNavS, linearFeature.EastingProNavS, UTMZone);
prepareKmlForUTMPointList(datasetFolder, "Projection" + parallelPathNotation,
linearFeature.NorthingProjection, linearFeature.EastingProjection, UTMZone);
prepareKmlForUTMPointPairs(datasetFolder, "ImageEndpoints" + parallelPathNotation, UTMZone,
linearFeature.NorthingImage1, linearFeature.EastingImage1,
linearFeature.NorthingImage2, linearFeature.EastingImage2);
* */
linearFeature.maximumMeasuredBank = maxBankDeg;
//add the linear feature just prepared fo the list of linear features
//there will be one prepared linear feature for each of the parallel paths
linearFeatures.Add(linearFeature);
}
//return the list of linear features prepared in this routine.
return linearFeatures;
}