public double distanceFromPoint2Line(PointD lineStart, double angleDeg, PointD pointIn)
{
//////////////////////////////////////////////////////////////////
// see http://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
//lineStart start point for the line
//angleDeg angle orientation of the line
//pointIn the point whose distance-to-line is required
//////////////////////////////////////////////////////////////////
//vector from pointIn to line start
PointD del = lineStart - pointIn;
//distance along input line to point at closest approach
double D = del.X * Math.Cos(angleDeg * Constants.Deg2Rad) + del.Y * Math.Sin(angleDeg * Constants.Deg2Rad);
double Vx = del.X - D * Math.Cos(angleDeg * Constants.Deg2Rad);
double Vy = del.Y - D * Math.Sin(angleDeg * Constants.Deg2Rad);
return Math.Sign(D) * Math.Sqrt(Vx * Vx + Vy * Vy);
}
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 PointD distanceFromPoint2Linefeature(
PointD pt, ref int index, ref double distanceToNextVertex,List<double> Northing, List<double> Easting )
{
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//determine the point-of-closest approach for an input point to a path defined as a set of ordered vertices
//output is the point along the path. The index of the nearest path vertex is input/returned and used as
//the starting point for the next searh to save time.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//pt = the point in proximity to the path feature
//index = the path vertex determined from the last entry
//Easting = path feature Easting values
//Northing = path feature Northing values
//return: the X-Y point on the lineFeature that is closest to pt
//detecting extrapolating -- two cases
// before first vertex -- index = -1 and distanceToNextVertex is distance to vertex 0
// after last vertex -- index = Count and distanceToNextVertex is -distance from final vertex
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//NOTE: the Northing and Easting are used as X and Y respectively in this procedure
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
PointD pca = new PointD();
bool foundThePCA = false;
double delX = 0.0, delY = 0.0, D = 0.0, mag1=0.0; ;
//assume that, on each call, the input point is farther along the input path
int count = 0;
while (!foundThePCA)
{
//current line segment
delX = Northing[index+1] - Northing[index];
delY = Easting[index+1] - Easting[index];
mag1 = Math.Sqrt(delX * delX + delY * delY);
//line from input point to segment start
double delPtX = pt.X - Northing[index];
double delPtY = pt.Y - Easting[index];
double mag2 = Math.Sqrt(delPtX * delPtX + delPtY * delPtY);
//must guard against mag2 geing zero ...
//normalized dot product of delPt and lineSegment
//projection of delPt onto del
if (mag1 > 0.0)
D = (delX * delPtX + delY * delPtY) / mag1;
else D = 0.0;
if (D < 0) //input point is before the current line segment -- need to decrement index
{
index--;
//Console.WriteLine(" decrementing index : " + index.ToString());
if (index < 0)
{
distanceToNextVertex = D; //distance to vertex 0 along the path
index = 0;
foundThePCA = true;
break;
}
}
else if (D > mag1) // input point is beyond the current line segment -- need to advance index
{
index++;
//Console.WriteLine(" incrementing index : " + index.ToString());
if (index > Easting.Count - 2)
{
distanceToNextVertex = -D; //distance to last along the path
index = Easting.Count-2;
//foundThePCA = true;
}
//break;
}
else if (D == 0) //input point is exactly on the vertex indicated by index
{
distanceToNextVertex = mag1;
foundThePCA = true;
break;
}
else
{
distanceToNextVertex = mag1 - D;
foundThePCA = true;
break;
}
count++;
if (count > 1000)
{
Console.WriteLine(" exiting distanceFromPoint2Linefeature on count");
break;
}
}
pca.X = Northing[index] + D * delX / mag1;
pca.Y = Easting[index] + D * delY / mag1;
//test for pca orthogonality
double test = (pt.X - pca.X) * delX + (pt.Y - pca.Y) * delY;
double dist = Math.Sqrt((pt.X - pca.X) * (pt.X - pca.X) + (pt.Y - pca.Y) *(pt.Y - pca.Y));
if (Math.Abs(test) > 0.01)
Console.WriteLine("distance = " + dist.ToString("F2") + " test for orthogonality = " + test.ToString("F4") );
return pca;
}
public bool intersectionOfTwoLines(PointD p1, PointD p2, PointD p3, PointD p4, PointD intersection)
{
//intersection of two lines formed by p1->p2 and p3->p4
//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;
}
public PointD FuturePointOnLineFeature(PointD _pt,
int ptIndex, 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;
//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 delX = Northing[ptIndex + 1] - Northing[ptIndex];
double delY = Easting[ptIndex + 1] - Easting[ptIndex];
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
{
ptIndex++;
//Console.WriteLine(" incrementing index for pointAhead : " + ptIndex.ToString());
if (ptIndex > (Easting.Count - 2))
{
ptIndex = Easting.Count - 2;
break;
}
//current line segment
delX = Northing[ptIndex + 1] - Northing[ptIndex];
delY = Easting[ptIndex + 1] - Easting[ptIndex];
lengthThisSegment = Math.Sqrt(delX * delX + delY * delY);
ptAhead_0.X = Northing[ptIndex];
ptAhead_0.Y = Easting[ptIndex];
//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(" future point index = " + ptIndex.ToString() + " pathToGoThisSegment = " + pathToGoThisSegment.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 int index, ref double distanceToNextVertex, List<double> Easting, List<double> Northing)
{
PointD pointOnPath = new PointD();
PointD ptEnd = new PointD();
PointD ptStart = new PointD();
PointD segmentEndpoint = new PointD();
segmentEndpoint.X = Northing[index + 1];
segmentEndpoint.Y = Easting[index + 1];
bool foundPointOnPath = false;
int count = 0; //used to get out of this procedure if it gets hung
bool pastLastPathPoint = false;
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.Sin(heading);
ptStart.Y = pt.Y - bigNumber * Math.Cos(heading);
ptEnd.X = pt.X - bigNumber * Math.Sin(heading);
ptEnd.Y = pt.Y + bigNumber * Math.Cos(heading);
//returns true only if we find an intersection point on the current line segment indicated by index
foundPointOnPath = intersectionOfTwoLines(
new PointD(Northing[index], Easting[index]), //startpoint of path segment
segmentEndpoint, //end point of path segment
ptStart, ptEnd, pointOnPath); //semi-infinite line and intersection
count++;
if (count > 100)
{
Console.WriteLine(" hung in pointOnPathOrthogonalToVelocityVector() -- breaking ");
break;
}
if (!foundPointOnPath)
{
index++; //if we dont find an intersection -- increment the index
//Console.WriteLine(" updating trajectory point : " + index.ToString());
if (index > (Easting.Count - 2)) //requires extrapolation beyond last endpoint
{
//if we move the trajectory past the last path vertex
//reset the segment endpoint to far beyond the last point but along the same heading
index = Easting.Count - 2;
segmentEndpoint.X = Northing[index] + 10.0 * (Northing[index + 1] - Northing[index]);
segmentEndpoint.Y = Easting[index] + 10.0 * (Easting[index + 1] - Easting[index]);
pastLastPathPoint = true;
}
else
{
segmentEndpoint.X = Northing[index + 1];
segmentEndpoint.Y = Easting[index + 1];
}
}
}
double dX = Northing[index + 1] - pointOnPath.X;
double dY = Easting[index + 1] - pointOnPath.Y;
distanceToNextVertex = Math.Sqrt(dX * dX + dY * dY);
if (pastLastPathPoint) distanceToNextVertex *= -1.0;
return pointOnPath;
}
