Пример #1
0
        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);
        }
Пример #2
0
        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;
        }
Пример #3
0
        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;
        }
Пример #4
0
        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;
        }
Пример #5
0
        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;
        }
Пример #6
0
        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;
        }