C# Utilities.utmpos类代码示例

 public utmpos(utmpos pos)
 {
     this.x = pos.x;
     this.y = pos.y;
     this.zone = pos.zone;
     this.Tag = null;
 }

        /// <summary>
        /// this is a debug function
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="tag"></param>
        static void addtomap(utmpos pos, string tag)
        {
            //tag = (no++).ToString();
            //polygons.Markers.Add(new GMapMarkerGoogleRed(pos.ToLLA()));// { ToolTipText = tag, ToolTipMode = MarkerTooltipMode.Always } );

            //map.ZoomAndCenterMarkers("polygons");

            //map.Invalidate();
        }

        /// <summary>
        /// this is a debug function
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="tag"></param>
        static void addtomap(utmpos pos, string tag)
        {
            if (tag == "M")
                return;

            polygons.Markers.Add(new GMapMarkerWP(pos.ToLLA(), tag));

            map.ZoomAndCenterMarkers("polygons");

            map.Invalidate();
        }

        public static List<PointLatLngAlt> CreateGrid(List<PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1,double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin = 0)
        {
            if (spacing < 10 && spacing != 0)
                spacing = 10;

            if (distance < 0.1)
                distance = 0.1;

            if (polygon.Count == 0)
                return new List<PointLatLngAlt>();

            
            // Make a non round number in case of corner cases
            if (minLaneSeparation != 0)
                minLaneSeparation += 0.5F;
            // Lane Separation in meters
            double minLaneSeparationINMeters = minLaneSeparation * distance;

            List<PointLatLngAlt> ans = new List<PointLatLngAlt>();

            // utm zone distance calcs will be done in
            int utmzone = polygon[0].GetUTMZone();

            // utm position list
            List<utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone);

            // close the loop if its not already
            if (utmpositions[0] != utmpositions[utmpositions.Count - 1])
                utmpositions.Add(utmpositions[0]); // make a full loop

            // get mins/maxs of coverage area
            Rect area = getPolyMinMax(utmpositions);

            // get initial grid

            // used to determine the size of the outer grid area
            double diagdist = area.DiagDistance();

            // somewhere to store out generated lines
            List<linelatlng> grid = new List<linelatlng>();
            // number of lines we need
            int lines = 0;

            // get start point middle
            double x = area.MidWidth;
            double y = area.MidHeight;

            addtomap(new utmpos(x, y, utmzone),"Base");

            // get left extent
            double xb1 = x;
            double yb1 = y;
            // to the left
            newpos(ref xb1, ref yb1, angle - 90, diagdist / 2 + distance);
            // backwards
            newpos(ref xb1, ref yb1, angle + 180, diagdist / 2 + distance);

            utmpos left = new utmpos(xb1, yb1, utmzone);

            addtomap(left, "left");

            // get right extent
            double xb2 = x;
            double yb2 = y;
            // to the right
            newpos(ref xb2, ref yb2, angle + 90, diagdist / 2 + distance);
            // backwards
            newpos(ref xb2, ref yb2, angle + 180, diagdist / 2 + distance);

            utmpos right = new utmpos(xb2, yb2, utmzone);

            addtomap(right,"right");

            // set start point to left hand side
            x = xb1;
            y = yb1;

            // draw the outergrid, this is a grid that cover the entire area of the rectangle plus more.
            while (lines < ((diagdist + distance * 2) / distance))
            {
                // copy the start point to generate the end point
                double nx = x;
                double ny = y;
                newpos(ref nx, ref ny, angle, diagdist + distance*2);

                linelatlng line = new linelatlng();
                line.p1 = new utmpos(x, y, utmzone);
                line.p2 = new utmpos(nx, ny, utmzone);
                line.basepnt = new utmpos(x, y, utmzone);
                grid.Add(line);

               // addtomap(line);

                newpos(ref x, ref y, angle + 90, distance);
                lines++;
            }

            // find intersections with our polygon

            // store lines that dont have any intersections
//.........这里部分代码省略.........

        static bool PointInPolygon(utmpos p, List<utmpos> poly)
        {
            utmpos p1, p2;
            bool inside = false;

            if (poly.Count < 3)
            {
                return inside;
            }
            utmpos oldPoint = new utmpos(poly[poly.Count - 1]);

            for (int i = 0; i < poly.Count; i++)
            {

                utmpos newPoint = new utmpos(poly[i]);

                if (newPoint.y > oldPoint.y)
                {
                    p1 = oldPoint;
                    p2 = newPoint;
                }
                else
                {
                    p1 = newPoint;
                    p2 = oldPoint;
                }

                if ((newPoint.y < p.y) == (p.y <= oldPoint.y)
                    && ((double)p.x - (double)p1.x) * (double)(p2.y - p1.y)
                    < ((double)p2.x - (double)p1.x) * (double)(p.y - p1.y))
                {
                    inside = !inside;
                }
                oldPoint = newPoint;
            }
            return inside;
        }

        static linelatlng findClosestLine(utmpos start, List<linelatlng> list, double minDistance, double angle)
        {
            // By now, just add 5.000 km to our lines so they are long enough to allow intersection
            double METERS_TO_EXTEND = 5000000;

            double perperndicularOrientation = AddAngle(angle, 90);

            // Calculation of a perpendicular line to the grid lines containing the "start" point
            /*
             *  --------------------------------------|------------------------------------------
             *  --------------------------------------|------------------------------------------
             *  -------------------------------------start---------------------------------------
             *  --------------------------------------|------------------------------------------
             *  --------------------------------------|------------------------------------------
             *  --------------------------------------|------------------------------------------
             *  --------------------------------------|------------------------------------------
             *  --------------------------------------|------------------------------------------
             */
            utmpos start_perpendicular_line = newpos(start, perperndicularOrientation, -METERS_TO_EXTEND);
            utmpos stop_perpendicular_line = newpos(start, perperndicularOrientation, METERS_TO_EXTEND);

            // Store one intersection point per grid line
            Dictionary<utmpos, linelatlng> intersectedPoints = new Dictionary<utmpos, linelatlng>();
            // lets order distances from every intersected point per line with the "start" point
            Dictionary<double, utmpos> ordered_min_to_max = new Dictionary<double, utmpos>();

            foreach (linelatlng line in list)
            {
                // Extend line at both ends so it intersecs for sure with our perpendicular line
                utmpos extended_line_start = newpos(line.p1, angle, -METERS_TO_EXTEND);
                utmpos extended_line_stop = newpos(line.p2, angle, METERS_TO_EXTEND);
                // Calculate intersection point
                utmpos p = FindLineIntersection(extended_line_start, extended_line_stop, start_perpendicular_line, stop_perpendicular_line);
                
                // Store it
                intersectedPoints[p] = line;

                // Calculate distances between interesected point and "start" (i.e. line and start)
                double distance_p = start.GetDistance(p);
                if (!ordered_min_to_max.ContainsKey(distance_p))
                    ordered_min_to_max.Add(distance_p, p);
            }
     
            // Acquire keys and sort them.
            List<double> ordered_keys = ordered_min_to_max.Keys.ToList();
            ordered_keys.Sort();

            // Lets select a line that is the closest to "start" point but "mindistance" away at least.
            // If we have only one line, return that line whatever the minDistance says
            double key = double.MaxValue;
            int i = 0;
            while (key == double.MaxValue && i < ordered_keys.Count)
            {
                if (ordered_keys[i] >= minDistance)
                    key = ordered_keys[i];
                i++;
            }

            // If no line is selected (because all of them are closer than minDistance, then get the farest one
            if (key == double.MaxValue)
                key = ordered_keys[ordered_keys.Count-1];

            // return line
            return intersectedPoints[ordered_min_to_max[key]];

        }

        static utmpos findClosestPoint(utmpos start, List<utmpos> list)
        {
            utmpos answer = utmpos.Zero;
            double currentbest = double.MaxValue;

            foreach (utmpos pnt in list)
            {
                double dist1 = start.GetDistance(pnt);

                if (dist1 < currentbest)
                {
                    answer = pnt;
                    currentbest = dist1;
                }
            }

            return answer;
        }

 /// <summary>
 /// from http://stackoverflow.com/questions/1119451/how-to-tell-if-a-line-intersects-a-polygon-in-c
 /// </summary>
 /// <param name="start1"></param>
 /// <param name="end1"></param>
 /// <param name="start2"></param>
 /// <param name="end2"></param>
 /// <returns></returns>
 public static utmpos FindLineIntersection(utmpos start1, utmpos end1, utmpos start2, utmpos end2)
 {
     double denom = ((end1.x - start1.x) * (end2.y - start2.y)) - ((end1.y - start1.y) * (end2.x - start2.x));
     //  AB & CD are parallel         
     if (denom == 0)
         return utmpos.Zero;
     double numer = ((start1.y - start2.y) * (end2.x - start2.x)) - ((start1.x - start2.x) * (end2.y - start2.y));
     double r = numer / denom;
     double numer2 = ((start1.y - start2.y) * (end1.x - start1.x)) - ((start1.x - start2.x) * (end1.y - start1.y));
     double s = numer2 / denom;
     if ((r < 0 || r > 1) || (s < 0 || s > 1))
         return utmpos.Zero;
     // Find intersection point      
     utmpos result = new utmpos();
     result.x = start1.x + (r * (end1.x - start1.x));
     result.y = start1.y + (r * (end1.y - start1.y));
     result.zone = start1.zone;
     return result;
 }

        // polar to rectangular
        static utmpos newpos(utmpos input, double bearing, double distance)
        {
            double degN = 90 - bearing;
            if (degN < 0)
                degN += 360;
            double x = input.x + distance * Math.Cos(degN * deg2rad);
            double y = input.y + distance * Math.Sin(degN * deg2rad);

            return new utmpos(x, y, input.zone);
        }

 public double GetDistance(utmpos b)
 {
     return Math.Sqrt(Math.Pow(Math.Abs(x - b.x), 2) + Math.Pow(Math.Abs(y - b.y), 2));
 }

        //http://en.wikipedia.org/wiki/Rapidly_exploring_random_tree
        // a*
        static List<PointLatLngAlt> FindPath(List<linelatlng> grid1, utmpos startposutm)
        {
            List<PointLatLngAlt> answer = new List<PointLatLngAlt>();

            List<linelatlng> closedset = new List<linelatlng>();
            List<linelatlng> openset = new List<linelatlng>(); // nodes to be travered
            Hashtable came_from = new Hashtable();
            List<linelatlng> grid = new List<linelatlng>();

            linelatlng start = new linelatlng() { p1 = startposutm, p2 = startposutm };

            grid.Add(start);
            grid.AddRange(grid1);
            openset.Add(start);

            Hashtable g_score = new Hashtable();
            Hashtable f_score = new Hashtable();
            g_score[start] = 0.0;
            f_score[start] = (double)g_score[start] + heuristic_cost_estimate(grid,0,start); // heuristic_cost_estimate(start, goal)

            linelatlng current = start;

            while (openset.Count > 0)
            {
                current = FindLowestFscore(g_score, openset); // lowest f_score
                openset.Remove(current);
                closedset.Add(current);
                foreach (var neighbor in neighbor_nodes(current, grid))
                {
                    double tentative_g_score = (double)g_score[current];

                    double dist1 = current.p1.GetDistance(neighbor.p1);
                    double dist2 = current.p1.GetDistance(neighbor.p2);
                    double dist3 = current.p2.GetDistance(neighbor.p1);
                    double dist4 = current.p2.GetDistance(neighbor.p2);

                    tentative_g_score += (dist1 + dist2 + dist3 + dist4) / 4;

                    tentative_g_score  += neighbor.p1.GetDistance(neighbor.p2);

                    //tentative_g_score += Math.Min(Math.Min(dist1, dist2), Math.Min(dist3, dist4));
                    //tentative_g_score += Math.Max(Math.Max(dist1, dist2), Math.Max(dist3, dist4));

                   // if (closedset.Contains(neighbor) && tentative_g_score >= (double)g_score[neighbor])
                   //     continue;

                    if (!closedset.Contains(neighbor) ||
                       tentative_g_score < (double)g_score[neighbor])
                    {
                        came_from[neighbor] = current;
                        g_score[neighbor] = tentative_g_score;
                        f_score[neighbor] = tentative_g_score + heuristic_cost_estimate(grid, tentative_g_score, neighbor);
                        Console.WriteLine("neighbor score: " + g_score[neighbor] + " " + f_score[neighbor]);
                        if (!openset.Contains(neighbor))
                            openset.Add(neighbor);
                    }
                }

            }

            // bad
            //linelatlng ans = FindLowestFscore(g_score, grid);

              //  foreach (var ans in grid)
            {
                List<linelatlng> list = reconstruct_path(came_from, current);
              //  list.Insert(0,current);
                //list.Remove(start);
                //list.Remove(start);
                Console.WriteLine("List " + list.Count + " " + g_score[current]);
               {
                   List<utmpos> temp = new List<utmpos>();
                   temp.Add(list[0].p1);
                   temp.Add(list[0].p2);
                   utmpos oldpos = findClosestPoint(startposutm, temp);

                   foreach (var item in list)
                   {
                       double dist1 = oldpos.GetDistance(item.p1);
                       double dist2 = oldpos.GetDistance(item.p2);
                       if (dist1 < dist2)
                       {
                           answer.Add(new PointLatLngAlt(item.p1));
                           answer.Add(new PointLatLngAlt(item.p2));
                           oldpos = item.p2;
                       }
                       else
                       {
                           answer.Add(new PointLatLngAlt(item.p2));
                           answer.Add(new PointLatLngAlt(item.p1));
                           oldpos = item.p1;
                       }
                   }
                   //return answer;
               }
            }

            List<PointLatLng> list2 = new List<PointLatLng>();
//.........这里部分代码省略.........

        static linelatlng findClosestLine(utmpos start, List<linelatlng> list)
        {
            linelatlng answer = list[0];
            double shortest = double.MaxValue;

            foreach (linelatlng line in list)
            {
                double ans1 = start.GetDistance(line.p1);
                double ans2 = start.GetDistance(line.p2);
                utmpos shorterpnt = ans1 < ans2 ? line.p1 : line.p2;

                if (shortest > start.GetDistance(shorterpnt))
                {
                    answer = line;
                    shortest = start.GetDistance(shorterpnt);
                }
            }

            return answer;
        }

        public static List<PointLatLngAlt> CreateGrid(List<PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double turn_radius, StartPosition startpos, bool shutter)
        {
            if (spacing < 10 && spacing != 0)
                spacing = 10;

            if (distance < 5)
                distance = 5;

            if (polygon.Count == 0)
                return new List<PointLatLngAlt>();

            double minLaneSeparationINMeters = 2 * turn_radius;
         /*
            if (minLaneSeparationINMeters < MinDistanceBetweenLines)
            {
                CustomMessageBox.Show("Sorry, minimal distance between lines is " + MinDistanceBetweenLines + " meters");
                minLaneSeparation = (float)(MinDistanceBetweenLines / distance + 1);
                minLaneSeparationINMeters = minLaneSeparation * distance;            
            }
        */

            List<PointLatLngAlt> ans = new List<PointLatLngAlt>();

            int utmzone = polygon[0].GetUTMZone();

            List<utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone);

            if (utmpositions[0] != utmpositions[utmpositions.Count - 1])
                utmpositions.Add(utmpositions[0]); // make a full loop

            Rect area = getPolyMinMax(utmpositions);

            // get initial grid

            // used to determine the size of the outer grid area
            double diagdist = area.DiagDistance();

            // somewhere to store out generated lines
            List<linelatlng> grid = new List<linelatlng>();
            // number of lines we need
            int lines = 0;

            // get start point bottom left
            double x = area.MidWidth;
            double y = area.MidHeight;

            addtomap(new utmpos(x, y, utmzone),"Base");

            // get left extent
            double xb1 = x;
            double yb1 = y;
            // to the left
            newpos(ref xb1, ref yb1, angle - 90, diagdist / 2 + distance);
            // backwards
            newpos(ref xb1, ref yb1, angle + 180, diagdist / 2 + distance);

            utmpos left = new utmpos(xb1, yb1, utmzone);

            addtomap(left, "left");

            // get right extent
            double xb2 = x;
            double yb2 = y;
            // to the right
            newpos(ref xb2, ref yb2, angle + 90, diagdist / 2 + distance);
            // backwards
            newpos(ref xb2, ref yb2, angle + 180, diagdist / 2 + distance);

            utmpos right = new utmpos(xb2, yb2, utmzone);

            addtomap(right,"right");

            // set start point to left hand side
            x = xb1;
            y = yb1;

            // draw the outergrid, this is a grid that cover the entire area of the rectangle plus more.
            while (lines < ((diagdist + distance * 2) / distance))
            {
                // copy the start point to generate the end point
                double nx = x;
                double ny = y;
                newpos(ref nx, ref ny, angle, diagdist + distance*2);

                linelatlng line = new linelatlng();
                line.p1 = new utmpos(x, y, utmzone);
                line.p2 = new utmpos(nx, ny, utmzone);
                line.basepnt = new utmpos(x, y, utmzone);
                grid.Add(line);

               // addtomap(line);

                newpos(ref x, ref y, angle + 90, distance);
                lines++;
            }

            // find intersections with our polygon

            // store lines that dont have any intersections
            List<linelatlng> remove = new List<linelatlng>();
//.........这里部分代码省略.........

        public static double cos_aob (utmpos a, utmpos o, utmpos b)
        {
            double A = a.GetDistance(o), B = b.GetDistance(o), C = a.GetDistance(b);

            return (- C * C + A * A + B * B) / (2 * A * B);
        }