public FlightPathLineGeometry( int _pathNumber, linearFeatureCoverageSummary _LFSum)
{
////////////////////////////////////////////////////////////////////////////////
//path: a set of smoothed trajectory points defined from the mission planned
//compute those variables that are constant across the path
////////////////////////////////////////////////////////////////////////////////
LFSum = _LFSum;
pathNumber = _pathNumber;
UTM2Geodetic utm = new UTM2Geodetic();
//set up the polygon (point list) math procedures
polyMath = new polygonMath(LFSum.paths[pathNumber].pathUTM);
//get the semi-infinite line extending beyond the path at the start and end
int count = LFSum.paths[pathNumber].pathGeoDeg.Count;
unitAwayFromEndUTM = new PointD();
unitAwayFromStartUTM = new PointD();
//semi-infinite line use for drawing a line extending beyond the path end
unitAwayFromEndUTM = LFSum.paths[pathNumber].pathUTM[count - 1] - LFSum.paths[pathNumber].pathUTM[count - 2];
unitAwayFromStartUTM = LFSum.paths[pathNumber].pathUTM[0] - LFSum.paths[pathNumber].pathUTM[1];
double delSMag = Math.Sqrt(unitAwayFromStartUTM.X * unitAwayFromStartUTM.X + unitAwayFromStartUTM.Y * unitAwayFromStartUTM.Y);
unitAwayFromStartUTM = unitAwayFromStartUTM / delSMag;
double delEMag = Math.Sqrt(unitAwayFromEndUTM.X * unitAwayFromEndUTM.X + unitAwayFromEndUTM.Y * unitAwayFromEndUTM.Y);
unitAwayFromEndUTM = unitAwayFromEndUTM / delEMag;
//semi-infinite line ate start and end in UTM coordinates
PointD semiInfiniteFLstartUTM = LFSum.paths[pathNumber].pathUTM[0] + 10000.0 * unitAwayFromStartUTM;
PointD semiInfiniteFLendUTM = LFSum.paths[pathNumber].pathUTM[count - 1] + 10000.0 * unitAwayFromEndUTM;
//convert to geodetic
semiInfiniteFLstartGeo = new PointD();
semiInfiniteFLendGeo = new PointD();
utm.UTMtoLL(semiInfiniteFLstartUTM, LFSum.UTMZone, ref semiInfiniteFLstartGeo);
utm.UTMtoLL(semiInfiniteFLendUTM, LFSum.UTMZone, ref semiInfiniteFLendGeo);
//compute the path length
pathlengthMeters = 0.0;
for (int i = 1; i < LFSum.paths[pathNumber].pathUTM.Count; i++)
{
double delX = LFSum.paths[pathNumber].pathUTM[i].X - LFSum.paths[pathNumber].pathUTM[i - 1].X;
double delY = LFSum.paths[pathNumber].pathUTM[i].Y - LFSum.paths[pathNumber].pathUTM[i - 1].Y;
double magDel = Math.Sqrt(delX * delX + delY * delY);
pathlengthMeters += magDel;
}
//number of expected photocenters
numPhotoCenters = (int)(pathlengthMeters / LFSum.photocenterSpacing + 1.0);
}
public List<endPoints> UpdateFlightLinesPerPriorFlownMissions(int missionNumber)
{
/////////////////////////////////////////////////////////////////////////////////////////////
//use the MissionUpdateFlightlines structure (fromn the constructor) to update the flight lines
//the new flight lines replace the old flight lines
//the initial prior flightline analysis provided a structure
//that contained only data from the flown missions.
//This procedure creates a replica of the mission plan flight lines,
//for a specific mission, that are adjusted to remove the prior flown lines (and segments)
/////////////////////////////////////////////////////////////////////////////////////////////
UTM2Geodetic utm = new UTM2Geodetic(); //needs to be in a utility procedure available to all in the solution
//test to see if pre-flown mission dataset contain this mission
int preFlownMissionIndex = 0; //mission index from the flightline analysis
bool thisMissionWasPreflown = false;
foreach (MissionUpdateFlightlines msnUpdate in projUpdate.msnUpdate)
{
if (missionNumber == msnUpdate.missionNumber) { thisMissionWasPreflown = true; break; }
preFlownMissionIndex++;
}
////////////////////////////////////////////////////////////////////////////////////////////
//this is the updates flight line dataset that replicates the data in the original plan
List<endPoints> FLUpdateList = new List<endPoints>(); //return value
////////////////////////////////////////////////////////////////////////////////////////////
//if this mission was not reflown -- just copy the old data to the replica
if (!thisMissionWasPreflown)
{
for (int iFL = 0; iFL < projSum.msnSum[missionNumber].numberOfFlightlines; iFL++)
FLUpdateList.Add(projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL]);
return FLUpdateList;
}
//if here, the mission was reflown -- generate the replica.
//NOTE: all flightlines were likely not preflown -- just a part of them.
//this is an index into the preflown-analysis structure indicating the reflown line
int nextFlownFL = 0;
//cycle through ALL flight lines for this mission
for (int iFL = 0; iFL < projSum.msnSum[missionNumber].numberOfFlightlines; iFL++)
{
bool thisFlightLineWasPreflown = false;
//the "if" below skips the checks on the remaining lines if we are beyond the nuber of reflown lines
if (nextFlownFL >= projUpdate.msnUpdate[preFlownMissionIndex].flightLineUpdate.Count) thisFlightLineWasPreflown = false;
//this is the test to see if we have reflown this line = iFL
else if (iFL == projUpdate.msnUpdate[preFlownMissionIndex].flightLineUpdate[nextFlownFL].FLNumber) thisFlightLineWasPreflown = true;
if (!thisFlightLineWasPreflown) //if not reflown -- just copy the old data
{
FLUpdateList.Add(projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL]);
}
else //create a new flightline dataset
{
// NOTE: we convert the initial geodetic ends to UTM for the now endpoint computations
// this is to maintain precision
//found a pre-flown flightline
PointD FLendGeo = projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL].end;
PointD FLstartGeo = projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL].start;
PointD FLendUTM = new PointD(0.0, 0.0);
PointD FLstartUTM = new PointD(0.0, 0.0);
//convert the original planned flight line ends to UTM -- could pass these in from the original plan
//NOTTE: maintain the same utm zone fro the mission planning -- else big trouble!!!
utm.LLtoUTM(FLstartGeo.Y * utm.Deg2Rad, FLstartGeo.X * utm.Deg2Rad, ref FLstartUTM.Y, ref FLstartUTM.X, ref projSum.UTMZone, true);
utm.LLtoUTM(FLendGeo.Y * utm.Deg2Rad, FLendGeo.X * utm.Deg2Rad, ref FLendUTM.Y, ref FLendUTM.X, ref projSum.UTMZone, true);
//below are the start and end photocenters as determined from the prior-flown mission analysis
//NOTE: the start is geodetically fixed -- e.g., at the south end for a NS flightline
int startPhotoCenter = projUpdate.msnUpdate[preFlownMissionIndex].flightLineUpdate[nextFlownFL].early;
int endPhotoCenter = projUpdate.msnUpdate[preFlownMissionIndex].flightLineUpdate[nextFlownFL].late;
PointD newStartUTM = new PointD(0.0, 0.0);
PointD newEndUTM = new PointD(0.0, 0.0);
// just a comparison of the computed and input flightline lengths ... it checks: JEK 1/26/2012
double FLMag1 = projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL].FLLengthMeters;
//double FLMag2 = Math.Sqrt((FLendUTM.X - FLstartUTM.X) * (FLendUTM.X - FLstartUTM.X) + (FLendUTM.Y - FLstartUTM.Y) * (FLendUTM.Y - FLstartUTM.Y));
//proportionally space the new photocenters along the origional flightine -- in UTM space
newStartUTM = FLstartUTM + (startPhotoCenter * projSum.downrangeTriggerSpacing / FLMag1) * (FLendUTM - FLstartUTM);
newEndUTM = FLstartUTM + (endPhotoCenter * projSum.downrangeTriggerSpacing / FLMag1) * (FLendUTM - FLstartUTM);
PointD newStartGeo = new PointD(0.0, 0.0);
PointD newEndGeo = new PointD(0.0, 0.0);
//now convert them back to geodetic
utm.UTMtoLL(newStartUTM.Y, newStartUTM.X, projSum.UTMZone, ref newStartGeo.Y, ref newStartGeo.X);
utm.UTMtoLL(newEndUTM.Y, newEndUTM.X, projSum.UTMZone, ref newEndGeo.Y, ref newEndGeo.X);
endPoints epts = new endPoints(); //temporary structure
//fill the temporary structure
//this used the 2D geometry and will work for NS or EW flight lines
epts.FLLengthMeters = (endPhotoCenter - startPhotoCenter) * projSum.downrangeTriggerSpacing;
epts.end = newEndGeo;
epts.start = newStartGeo;
//this is the global project flight line number -- not a local number used for this mission
epts.FlightLineNumber = projSum.msnSum[missionNumber].FlightLinesCurrentPlan[iFL].FlightLineNumber;
//below is important to allow flight line updates that include partial flown lines.
//all photocenters are given a unique name in the original flight plan
//we must keep this original name for the photocenters
//the names are based on the distance from the geodetic fixed otiginal start location
// so we have to keep the offset for the updated flight lines from the original plan
epts.photoCenterOffset = startPhotoCenter;
//fill the replical flight line structure
FLUpdateList.Add(epts);
//increment the index into the preflown flightline analysis structure
nextFlownFL++;
} //end of filling the new flightline record
} //end of filling the individual flight lines (iFL)
return FLUpdateList; //filled replica of the original flightplan fllightlines for this mission
}
public void LLtoUTM(PointD latLon,
ref PointD UTM, ref String UTMDesignation, bool usePresetUTMZone)
{
UTM = new PointD(0.0, 0.0);
LLtoUTM(latLon.X, latLon.Y, ref UTM.Y, ref UTM.X, ref UTMDesignation, usePresetUTMZone);
}
//overloaded conversion to allow PointD
public void UTMtoLL(PointD UTM, String UTMZone, ref PointD pointGeoRad)
{
UTMtoLL(UTM.Y, UTM.X, UTMZone, ref pointGeoRad.Y, ref pointGeoRad.X);
}
public PointD FuturePointOnLineFeature(PointD _pt,
double ptDistanceToNextVertex, double distanceAhead,
List<double> Easting, List<double> Northing)
{
//////////////////////////////////////////////////////////////////////////////////////////////
//determine a point on the input path that is a prescribed distance ahead of the input point
//////////////////////////////////////////////////////////////////////////////////////////////
//pt = input point on the line feature (assumed to be on the path)
//ptIndex = index of the vertex prior to pt (prior computed)
//ptDistanceToNextVertex = distance to next vertex for input point (prior computed)
//distanceAhead = distance ahead along the linear feature (path)
//return: = the X-Y point on the lineFeature ahead of the current point
PointD ptAhead = new PointD();
/// do this because pt is changed below
PointD pt = new PointD();
pt.X = _pt.X;
pt.Y = _pt.Y;
int indexForPointAhead = indexForAircraft;
//do this to cover the case where the future point is in the same segment as the last entry
//compute the segment length for the segment containing the prior futurePoint
double delY = Northing[indexForPointAhead + 1] - Northing[indexForPointAhead];
double delX = Easting[indexForPointAhead + 1] - Easting[indexForPointAhead];
double lengthThisSegment = Math.Sqrt(delX * delX + delY * delY);
//useablePathLength includes remainder of prior-used segment + new added segments
double useablePathLength = ptDistanceToNextVertex;
double pathToGoThisSegment = distanceAhead;
PointD ptAhead_0 = new PointD();
ptAhead_0.X = pt.X;
ptAhead_0.Y = pt.Y;
int count = 0;
bool foundSegmentContainingFuturePoint = false;
while (!foundSegmentContainingFuturePoint)
{
//if future point is on segment ptIndex, get out of the while loop
if (useablePathLength > distanceAhead)
{
foundSegmentContainingFuturePoint = true;
}
//else increment the vertex and compute distance along path to that vertex
else
{
indexForPointAhead++;
//Console.WriteLine(" incrementing index for pointAhead : " + ptIndex.ToString());
if (indexForPointAhead > (Easting.Count - 2))
{
indexForPointAhead = Easting.Count - 2;
break;
}
//current line segment
delY = Northing[indexForPointAhead + 1] - Northing[indexForPointAhead];
delX = Easting[indexForPointAhead + 1] - Easting[indexForPointAhead];
lengthThisSegment = Math.Sqrt(delX * delX + delY * delY);
ptAhead_0.Y = Northing[indexForPointAhead];
ptAhead_0.X = Easting[indexForPointAhead];
//add the new segment path length to the useable path length
useablePathLength += lengthThisSegment;
pathToGoThisSegment = distanceAhead - useablePathLength + lengthThisSegment;
}
count++;
if (count > 100)
{
Console.WriteLine(" exiting FuturePointOnLineFeature() on count");
break;
}
}
//Console.WriteLine("index = " + indexForPointAhead.ToString() + " pathToGoThisSegment = " +
// pathToGoThisSegment.ToString("F1") + " useableLength " + useablePathLength.ToString("F1"));
ptAhead.X = ptAhead_0.X + pathToGoThisSegment * delX / lengthThisSegment;
ptAhead.Y = ptAhead_0.Y + pathToGoThisSegment * delY / lengthThisSegment;
return ptAhead;
}
public PointD pointOnPathOrthogonalToVelocityVector(double heading,
PointD pt, ref double distanceToNextVertex, ref double distanceFromLastVertex)
{
///////////////////////////////////////////////////////////////////////////////////////////////////
//determine a point on the path segment that is orthognal to an input velocity vector
//inputs
// heading the heading of the velocity vector (radians)
// pt the input point to nearest point on path orthogonal to velocity
//output
// PointD result the point on the path closest to the input point
// distanceToNextVertex distance along the current segment to the next path vertex
///////////////////////////////////////////////////////////////////////////////////////////////////
PointD pointOnPath = new PointD();
PointD ptEnd = new PointD();
PointD ptStart = new PointD();
PointD segmentEndPoint = new PointD();
segmentEndPoint.X = Easting[indexForAircraft + 1];
segmentEndPoint.Y = Northing[indexForAircraft + 1];
double delSX = 0.0, delSY = 0.0;
bool foundPointOnPath = false;
int count = 0; //used to get out of this procedure if it gets hung
while (!foundPointOnPath)
{
//form semi-infinite line orthogonal to the velocity vector
//and passing through pt
double bigNumber = 1000000.0;
//ptStart -> ptEnd form a semi-infinite vector orthogonal to the velocity vector and passing through pt
ptStart.X = pt.X + bigNumber * Math.Cos(heading); //heading is east of north -- X is to the east
ptStart.Y = pt.Y - bigNumber * Math.Sin(heading); //heading is east of north -- Y is to the north
ptEnd.X = pt.X - bigNumber * Math.Cos(heading);
ptEnd.Y = pt.Y + bigNumber * Math.Sin(heading);
//if the input point is before the first path point -- extend the first segment backwards
PointD segmentStartPoint = new PointD(Easting[indexForAircraft], Northing[indexForAircraft]);
if (indexForAircraft == 0)
{
delSX = Easting[indexForAircraft + 1] - Easting[indexForAircraft];
delSY = Northing[indexForAircraft + 1] - Northing[indexForAircraft];
segmentStartPoint.X += bigNumber * (Easting[indexForAircraft] - Easting[indexForAircraft + 1]);
segmentStartPoint.Y += bigNumber * (Northing[indexForAircraft] - Northing[indexForAircraft + 1]);
}
else if (indexForAircraft == Easting.Count - 2)
{
segmentEndPoint.X += bigNumber * (Easting[indexForAircraft + 1] - Easting[indexForAircraft]);
segmentEndPoint.Y += bigNumber * (Northing[indexForAircraft + 1] - Northing[indexForAircraft]);
}
//returns true only if we find an intersection point on the current line segment indicated by index
foundPointOnPath = intersectionOfTwoLines(
segmentStartPoint, //startpoint of path segment
segmentEndPoint, //end point of path segment
ptStart, ptEnd, pointOnPath); //semi-infinite line and intersection
count++;
if (count > Northing.Count-1)
{
Console.WriteLine(" hung in pointOnPathOrthogonalToVelocityVector() -- breaking ");
break;
}
if (!foundPointOnPath)
{
indexForAircraft++; //if we dont find an intersection -- increment the index
if (indexForAircraft >= Easting.Count - 2) indexForAircraft = Easting.Count - 2;
//Console.WriteLine(" updating trajectory point : " + index.ToString());
segmentEndPoint.Y = Northing[indexForAircraft + 1];
segmentEndPoint.X = Easting[indexForAircraft + 1];
}
}
double dY1 = Northing[indexForAircraft + 1] - pointOnPath.Y;
double dX1 = Easting[indexForAircraft + 1] - pointOnPath.X;
distanceToNextVertex = Math.Sqrt(dX1 * dX1 + dY1 * dY1);
double dY2 = pointOnPath.Y - Northing[indexForAircraft];
double dX2 = pointOnPath.X - Easting[indexForAircraft];
distanceFromLastVertex = Math.Sqrt(dX2 * dX2 + dY2 * dY2);
if (dX2 * delSX + dY2 * delSY < 0) distanceFromLastVertex *= -1.0;
return pointOnPath;
}
public PointD pointAlongPathFromStart(double distanceFromStart)
{
////////////////////////////////////////////////////////////////////////////////////////////////
// find a point along a path formed from points that is distanceFromStart from the start point
////////////////////////////////////////////////////////////////////////////////////////////////
double distanceAlongPath = 0;
double distanceAlongPathAtPriorPoint = 0;
PointD pointAlongPath = new PointD();
int i = 1; double delE = 0.0; double delN = 0.0; double segmentLength = 0.0;
for (i = 1; i < Easting.Count; i++)
{
delE = Easting[i] - Easting[i - 1];
delN = Northing[i] - Northing[i - 1];
segmentLength = Math.Sqrt(delE * delE + delN * delN);
distanceAlongPath += segmentLength; //distance to next path point
if (distanceFromStart <= distanceAlongPath) break;
distanceAlongPathAtPriorPoint = distanceAlongPath;
}
//i point along path is past the input point -- so use i-1 & i point to interpolate
double distanceToGo = distanceFromStart - distanceAlongPathAtPriorPoint;
pointAlongPath.X = Easting[i - 1] + distanceToGo * delE / segmentLength;
pointAlongPath.Y = Northing[i - 1] + distanceToGo * delN / segmentLength;
return pointAlongPath;
}
public double LOSRatetoPointAlongPath(
PointD currentVehicleLocation, PointD velocityVector, double distanceToPoint,
ref double distanceAlongPath, ref double velocityAlongPath, ref double headingToPath, ref int currentVertex)
{
////////////////////////////////////////////////////////////////////////////////////
// coordinate axes: careful!! X is to the east and Y is to the north here
///////////////////////////////////////////////////////////////////////////////////////////
//currentVehicleLocation current location of the vehicle in UTM
//velocityVector current velocity vector in m/sec
//distanceToPoint distance ahead along the path that is the target
//output: the signed LOS rate to the point ahead
//nearestPointOnPath the PointD on the path that is closest to the input point
///////////////////////////////////////////////////////////////////////////////////////////
double LOSRate = 0.0;
double heading = Math.Atan2(velocityVector.X, velocityVector.Y);
double distanceToNextVertex = 0;
double distanceFromLastVertex = 0;
PointD nearestPointOnPath = new PointD();
try
{
//get the nearest point along the path that is most orthogonal to the velocity vector
nearestPointOnPath = pointOnPathOrthogonalToVelocityVector(
heading, currentVehicleLocation, ref distanceToNextVertex, ref distanceFromLastVertex);
}
catch
{
//int a = 0;
}
PointD pAhead = FuturePointOnLineFeature(
nearestPointOnPath, distanceToNextVertex, distanceToPoint,
Easting, Northing);
//locate the aircraft distance along the path
distanceAlongPath = alongPathDistanceAtVertex[indexForAircraft] + distanceFromLastVertex;
//Console.WriteLine(" distanceAlogPath = " + distanceAlongPath.ToString() );
//compute the Line-Of-Sight rate between the platform and the target point
//the gammaDot will be commanded to -3*LOSRate
double delX = pAhead.X - currentVehicleLocation.X;
double delY = pAhead.Y - currentVehicleLocation.Y;
double LOSAngle = Math.Atan2(delX, delY);
LOSRate = -Math.Cos(LOSAngle) * Math.Cos(LOSAngle) * (velocityVector.X / delY - delX / (delY * delY) * velocityVector.Y);
//compute the vehicle heading relative to the closest path segment
// heading: cross product of the velocity with the unit vector along path segment
// velocityAlongPath: dot product of velocity vector and nearest path segment
PointD delPathSegment = new PointD();
delPathSegment.X = Easting[indexForAircraft + 1] - Easting[indexForAircraft];
delPathSegment.Y = Northing[indexForAircraft + 1] - Northing[indexForAircraft];
double delMag = Math.Sqrt(delPathSegment.X * delPathSegment.X + delPathSegment.Y * delPathSegment.Y);
double velocityAcrossPath = (delPathSegment.X * velocityVector.Y - velocityVector.X * delPathSegment.Y)/delMag;
velocityAlongPath = (delPathSegment.X * velocityVector.X + velocityVector.Y * delPathSegment.Y) / delMag;
headingToPath = Math.Atan2(velocityAcrossPath, velocityAlongPath);
currentVertex = indexForAircraft;
return LOSRate;
}
public bool intersectionOfTwoLines(PointD p1, PointD p2, PointD p3, PointD p4, PointD intersection)
{
//intersection of two lines formed by p1->p2 and p3->p4
//return true of the intersection is between the endpoints of both lines
//return false otherwise
//see this site for equations: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
double denom1 = (p4.Y - p3.Y) * (p2.X - p1.X) - (p4.X - p3.X) * (p2.Y - p1.Y);
if (Math.Abs(denom1) < 1.0e-60) return false; //lines are parallel
double ua = ((p4.X - p3.X) * (p1.Y - p3.Y) - (p4.Y - p3.Y) * (p1.X - p3.X)) / denom1;
double ub = ((p2.X - p1.X) * (p1.Y - p3.Y) - (p2.Y - p1.Y) * (p1.X - p3.X)) / denom1;
if (Math.Abs(ua) < 0.00001) ua = 0.0;
if (Math.Abs(ub) < 0.00001) ub = 0.0;
if (ua < 0 || ua > 1.0) return false; //intersection outside line p3-p4
if (ub < 0 || ub > 1.0) return false; //intersection outside line p1-p2
double deleasting = ua * (p2.X - p1.X); //double deleastingPix = deleasting/mosaicGeo->deasting;
double delnorthing = ua * (p2.Y - p1.Y); //double delnorthingPix = delnorthing/mosaicGeo->dnorthing;
intersection.X = p1.X + deleasting;
intersection.Y = p1.Y + delnorthing;
return true;
}