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; }