Ejemplo n.º 1
0
        static void addtomap(linelatlng pos)
        {
            List<PointLatLng> list = new List<PointLatLng>();
            list.Add(pos.p1.ToLLA());
            list.Add(pos.p2.ToLLA());

            polygons.Routes.Add(new GMapRoute(list, "test") { Stroke = new System.Drawing.Pen(System.Drawing.Color.Yellow, 4) });

            map.ZoomAndCenterRoutes("polygons");
        }
Ejemplo n.º 2
0
        static void addtomap(linelatlng pos)
        {
            return;
            //List<PointLatLng> list = new List<PointLatLng>();
            //list.Add(pos.p1.ToLLA());
            //list.Add(pos.p2.ToLLA());

         //   polygons.Routes.Add(new GMapRoute(list, "test") { Stroke = new System.Drawing.Pen(System.Drawing.Color.Yellow,4) });
            
            //.Markers.Add(new GMapMarkerGoogleRed(pnt));

            //map.ZoomAndCenterRoutes("polygons");

           // map.Invalidate();
        }
Ejemplo n.º 3
0
        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
            List<linelatlng> remove = new List<linelatlng>();

            int gridno = grid.Count;

            // cycle through our grid
            for (int a = 0; a < gridno; a++)
            {
                double closestdistance = double.MaxValue;
                double farestdistance = double.MinValue;

                utmpos closestpoint = utmpos.Zero;
                utmpos farestpoint = utmpos.Zero;

                // somewhere to store our intersections
                List<utmpos> matchs = new List<utmpos>();

                int b = -1;
                int crosses = 0;
                utmpos newutmpos = utmpos.Zero;
                foreach (utmpos pnt in utmpositions)
                {
                    b++;
                    if (b == 0)
                    {
                        continue;
                    }
                    newutmpos = FindLineIntersection(utmpositions[b - 1], utmpositions[b], grid[a].p1, grid[a].p2);
                    if (!newutmpos.IsZero)
                    {
                        crosses++;
                        matchs.Add(newutmpos);
                        if (closestdistance > grid[a].p1.GetDistance(newutmpos))
                        {
                            closestpoint.y = newutmpos.y;
                            closestpoint.x = newutmpos.x;
                            closestpoint.zone = newutmpos.zone;
                            closestdistance = grid[a].p1.GetDistance(newutmpos);
                        }
                        if (farestdistance < grid[a].p1.GetDistance(newutmpos))
                        {
                            farestpoint.y = newutmpos.y;
                            farestpoint.x = newutmpos.x;
                            farestpoint.zone = newutmpos.zone;
                            farestdistance = grid[a].p1.GetDistance(newutmpos);
                        }
                    }
                }
                if (crosses == 0) // outside our polygon
                {
                    if (!PointInPolygon(grid[a].p1, utmpositions) && !PointInPolygon(grid[a].p2, utmpositions))
                        remove.Add(grid[a]);
                }
                else if (crosses == 1) // bad - shouldnt happen
                {

                }
                else if (crosses == 2) // simple start and finish
                {
                    linelatlng line = grid[a];
                    line.p1 = closestpoint;
                    line.p2 = farestpoint;
                    grid[a] = line;
                }
                else // multiple intersections
                {
                    linelatlng line = grid[a];
                    remove.Add(line);

                    while (matchs.Count > 1)
                    {
                        linelatlng newline = new linelatlng();

                        closestpoint = findClosestPoint(closestpoint, matchs);
                        newline.p1 = closestpoint;
                        matchs.Remove(closestpoint);

                        closestpoint = findClosestPoint(closestpoint, matchs);
                        newline.p2 = closestpoint;
                        matchs.Remove(closestpoint);

                        newline.basepnt = line.basepnt;

                        grid.Add(newline);
                    }
                }
            }

            // cleanup and keep only lines that pass though our polygon
            foreach (linelatlng line in remove)
            {
                grid.Remove(line);
            }

            // debug
            foreach (linelatlng line in grid)
            {
                addtomap(line);
            }

            if (grid.Count == 0)
                return ans;

            utmpos startposutm;

            switch (startpos)
            {
                default:
                case StartPosition.Home:
                    startposutm = new utmpos(Host2.cs.HomeLocation);
                    break;
                case StartPosition.BottomLeft:
                    startposutm = new utmpos(area.Left, area.Bottom, utmzone);
                    break;
                case StartPosition.BottomRight:
                    startposutm = new utmpos(area.Right, area.Bottom, utmzone);
                    break;
                case StartPosition.TopLeft:
                    startposutm = new utmpos(area.Left, area.Top, utmzone);
                    break;
                case StartPosition.TopRight:
                    startposutm = new utmpos(area.Right, area.Top, utmzone);
                    break;

            }

            // find closest line point to startpos
            linelatlng closest = findClosestLine(startposutm, grid, 0 /*Lane separation does not apply to starting point*/, angle);

            utmpos lastpnt;

            // get the closes point from the line we picked
            if (closest.p1.GetDistance(startposutm) < closest.p2.GetDistance(startposutm))
            {
                lastpnt = closest.p1;
            }
            else
            {
                lastpnt = closest.p2;
            }

            while (grid.Count > 0)
            {
                // for each line, check which end of the line is the next closest
                if (closest.p1.GetDistance(lastpnt) < closest.p2.GetDistance(lastpnt))
                {
                    utmpos newstart = newpos(closest.p1, angle, -leadin);

                    addtomap(newstart, "S");
                    ans.Add(newstart);

                    if (spacing > 0)
                    {
                        for (int d = (int)(spacing - ((closest.basepnt.GetDistance(closest.p1)) % spacing));
                            d < (closest.p1.GetDistance(closest.p2));
                            d += (int)spacing)
                        {
                            double ax = closest.p1.x;
                            double ay = closest.p1.y;

                            newpos(ref ax, ref ay, angle, d);
                            addtomap(new utmpos(ax,ay,utmzone),"M");
                            ans.Add((new utmpos(ax, ay, utmzone) { Tag = "M" }));
                        }
                    }


                    utmpos newend = newpos(closest.p2, angle, overshoot1);
                    addtomap(newend, "E");
                    ans.Add(newend);

                    lastpnt = closest.p2;

                    grid.Remove(closest);
                    if (grid.Count == 0)
                        break;

                    closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle);
                }
                else
                {
                    utmpos newstart = newpos(closest.p2, angle, leadin);
                    addtomap(newstart, "E");
                    ans.Add(newstart);

                    if (spacing > 0)
                    {
                        for (int d = (int)((closest.basepnt.GetDistance(closest.p2)) % spacing);
                            d < (closest.p1.GetDistance(closest.p2));
                            d += (int)spacing)
                        {
                            double ax = closest.p2.x;
                            double ay = closest.p2.y;

                            newpos(ref ax, ref ay, angle, -d);
                            addtomap(new utmpos(ax, ay, utmzone), "M");
                            ans.Add((new utmpos(ax, ay, utmzone) { Tag = "M" }));
                        }
                    }

                    utmpos newend = newpos(closest.p1, angle, -overshoot2);
                 //   if (overshoot2 > 0)
                 //       ans.Add(new utmpos(closest.p1) { Tag = "M" });
                    addtomap(newend, "E");
                    ans.Add(newend);

                    lastpnt = closest.p1;

                    grid.Remove(closest);
                    if (grid.Count == 0)
                        break;
                    closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle);
                }
            }

            // set the altitude on all points
            ans.ForEach(plla => { plla.Alt = altitude; });

            return ans;
        }
Ejemplo n.º 4
0
        static List<linelatlng> reconstruct_path(Hashtable came_from, linelatlng current_node)
        {
            List<linelatlng> ans = new List<linelatlng>();
            if (came_from.ContainsKey(current_node))
            {

                ans.AddRange(reconstruct_path(came_from, (linelatlng)came_from[current_node]));
                ans.Add((linelatlng)came_from[current_node]);
                return ans;
            }
            else
            {
                ans.Add(current_node);
                return ans;
            }
        }
Ejemplo n.º 5
0
        private static List<linelatlng> neighbor_nodes(linelatlng current, List<linelatlng> grid)
        {
            List<linelatlng> neighbors = new List<linelatlng>();

            foreach (var item in grid)
            {
               // if (item.Equals(current))
              //      continue;

                neighbors.Add(item);
            }

            return neighbors;
        }
Ejemplo n.º 6
0
        static double heuristic_cost_estimate(List<linelatlng> grid, double sofar, linelatlng current_node)
        {
            double ans = 0;

            linelatlng lastx = grid[0];

            grid.ForEach(x =>
            {
                ans += x.p1.GetDistance(x.p2);
                ans += x.p1.GetDistance(lastx.p1);
                lastx = x;
            });

            return ans - sofar * 0.95;
        }
Ejemplo n.º 7
0
        //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>();

            answer.ForEach(x => { list2.Add(x); });

            GMapPolygon wppoly = new GMapPolygon(list2, "Grid");

            Console.WriteLine("dist " + (wppoly.Distance));

            return answer;
        }
Ejemplo n.º 8
0
        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>();

            int gridno = grid.Count;

            // cycle through our grid
            for (int a = 0; a < gridno; a++)
            {
                double closestdistance = double.MaxValue;
                double farestdistance = double.MinValue;

                utmpos closestpoint = utmpos.Zero;
                utmpos farestpoint = utmpos.Zero;

                // somewhere to store our intersections
                List<utmpos> matchs = new List<utmpos>();

                int b = -1;
                int crosses = 0;
                utmpos newutmpos = utmpos.Zero;
                foreach (utmpos pnt in utmpositions)
                {
                    b++;
                    if (b == 0)
                    {
                        continue;
                    }
                    newutmpos = FindLineIntersection(utmpositions[b - 1], utmpositions[b], grid[a].p1, grid[a].p2);
                    if (!newutmpos.IsZero)
                    {
                        crosses++;
                        matchs.Add(newutmpos);
                        if (closestdistance > grid[a].p1.GetDistance(newutmpos))
                        {
                            closestpoint.y = newutmpos.y;
                            closestpoint.x = newutmpos.x;
                            closestpoint.zone = newutmpos.zone;
                            closestdistance = grid[a].p1.GetDistance(newutmpos);
                        }
                        if (farestdistance < grid[a].p1.GetDistance(newutmpos))
                        {
                            farestpoint.y = newutmpos.y;
                            farestpoint.x = newutmpos.x;
                            farestpoint.zone = newutmpos.zone;
                            farestdistance = grid[a].p1.GetDistance(newutmpos);
                        }
                    }
                }
                if (crosses == 0) // outside our polygon
                {
                    if (!PointInPolygon(grid[a].p1, utmpositions) && !PointInPolygon(grid[a].p2, utmpositions))
                        remove.Add(grid[a]);
                }
                else if (crosses == 1) // bad - shouldnt happen
                {

                }
                else if (crosses == 2) // simple start and finish
                {
                    linelatlng line = grid[a];
                    line.p1 = closestpoint;
                    line.p2 = farestpoint;
                    grid[a] = line;
                }
                else // multiple intersections
                {
                    linelatlng line = grid[a];
                    remove.Add(line);

                    while (matchs.Count > 1)
                    {
                        linelatlng newline = new linelatlng();

                        closestpoint = findClosestPoint(closestpoint, matchs);
                        newline.p1 = closestpoint;
                        matchs.Remove(closestpoint);

                        closestpoint = findClosestPoint(closestpoint, matchs);
                        newline.p2 = closestpoint;
                        matchs.Remove(closestpoint);

                        newline.basepnt = line.basepnt;

                        grid.Add(newline);
                    }
                }
            }

            foreach (linelatlng line in remove)
            {
                grid.Remove(line);
            }

            foreach (linelatlng line in grid)
            {
                addtomap(line);
            }

            if (grid.Count == 0)
                return ans;

            utmpos startposutm;
         
            switch (startpos)
            {
                default:
                case StartPosition.Home:
                    startposutm = new utmpos(Host2.cs.HomeLocation);
                    break;
                case StartPosition.BottomLeft:
                    startposutm = new utmpos(area.Left, area.Bottom, utmzone);
                    break;
                case StartPosition.BottomRight:
                    startposutm = new utmpos(area.Right, area.Bottom, utmzone);
                    break;
                case StartPosition.TopLeft:
                    startposutm = new utmpos(area.Left, area.Top, utmzone);
                    break;
                case StartPosition.TopRight:
                    startposutm = new utmpos(area.Right, area.Top, utmzone);
                    break;

            }

            //return 
          //      FindPath(grid, startposutm);

            // find closest line point to home
            Tuple <linelatlng, bool> closest_ = findClosestLine(startposutm, grid, 0 /*Lane separation does not apply to starting point*/, angle);
            linelatlng closest = closest_.Item1;
            bool isFurtherMinDintace = closest_.Item2;
            bool oldIsFurtherMinDintace = closest_.Item2; 
            utmpos lastpnt;

            if (closest.p1.GetDistance(startposutm) < closest.p2.GetDistance(startposutm))
            {
                lastpnt = closest.p1;
            }
            else
            {
                lastpnt = closest.p2;
            }

            utmpos newstart = utmpos.Zero, newend = utmpos.Zero, oldstart = newstart, oldend = newend;
            int i = 0;
            bool flightForward = closest.p1.GetDistance(lastpnt) < closest.p2.GetDistance(lastpnt);
            List<PointLatLngAlt> tmp = new List<PointLatLngAlt>();
            List<PointLatLngAlt> oldTmp;

            while (grid.Count > 0)
            {
                oldend = newend;
                oldstart = newstart;
                oldIsFurtherMinDintace = isFurtherMinDintace;
                oldTmp = tmp;
                tmp = new List<PointLatLngAlt>();

                if (flightForward)
                {
                    newstart = newpos(closest.p1, 180 + angle, 2 * turn_radius);

                    if (spacing > 0)
                    {
                        for (int d = (int)(spacing - ((closest.basepnt.GetDistance(closest.p1)) % spacing));
                            d < (closest.p1.GetDistance(closest.p2));
                            d += (int)spacing)
                        {
                            double ax = closest.p1.x;
                            double ay = closest.p1.y;

                            newpos(ref ax, ref ay, angle, d);
                            addtomap(new utmpos(ax,ay,utmzone),"M");
                            tmp.Add((new utmpos(ax, ay, utmzone) { Tag = "M"}));
                          //ans.Add((new utmpos(ax, ay, utmzone) { Tag = "M" }));

                          //  if (shutter.ToLower().StartsWith("y"))
                              //  AddDigicamControlPhoto();
                        }
                    }


                    newend = newpos(closest.p2, angle, 2 * turn_radius);
                  //  if (overshoot1 > 0)
                   //     ans.Add(new utmpos(closest.p2) { Tag = "M" });

                    lastpnt = closest.p2;

                    grid.Remove(closest);
                    if (grid.Count != 0) { 
                        closest_ = findClosestLine(newend, grid, minLaneSeparationINMeters, angle); 
                        closest = closest_.Item1;
                        isFurtherMinDintace = closest_.Item2;
                    }
                }
                else
                {
                    newstart = newpos(closest.p2, 180 + angle, -2 * turn_radius);
                    
                    if (spacing > 0)
                    {
                        for (int d = (int)((closest.basepnt.GetDistance(closest.p2)) % spacing);
                            d < (closest.p1.GetDistance(closest.p2));
                            d += (int)spacing)
                        {
                            double ax = closest.p2.x;
                            double ay = closest.p2.y;

                            newpos(ref ax, ref ay, angle, -d);
                            addtomap(new utmpos(ax, ay, utmzone), "M");
                            tmp.Add((new utmpos(ax, ay, utmzone) { Tag = "M" }));
                            //ans.Add((new utmpos(ax, ay, utmzone) { Tag = "M" }));

                           // if (shutter.ToLower().StartsWith("y"))
                            //    AddDigicamControlPhoto();
                        }
                    }

                    newend = newpos(closest.p1, angle, -2 * turn_radius);
                 //   if (overshoot2 > 0)
                 //       ans.Add(new utmpos(closest.p1) { Tag = "M" });
                    
                    lastpnt = closest.p1;

                    grid.Remove(closest);
                    if (grid.Count != 0)
                    {
                        closest_ = findClosestLine(newend, grid, minLaneSeparationINMeters, angle);
                        closest = closest_.Item1;
                        isFurtherMinDintace = closest_.Item2;
                    }
                }

                if (i != 0)
                {
                    
                    int sign = flightForward ? -1 : 1;
                    double addDist = oldend.GetDistance(newstart) * cos_aob(oldend, newstart, newend);

                    if (addDist < 0)
                    {
                        newstart = newpos(newstart, 180 + angle, sign * addDist);
                    }
                    else
                    {
                        oldend = newpos(oldend, angle, sign * addDist);
                    }

                    
                    addtomap(oldstart, "S");
                    ans.Add(oldstart);
                    for (int j = 0; j < oldTmp.Count; j++)
                        ans.Add(oldTmp[j]);
                    addtomap(oldend, "E");
                    ans.Add(oldend);
                    oldTmp.Clear();

                    //flying around in a circle
                    if (!oldIsFurtherMinDintace)
                    {
                        double tmpAngle = angle;
                        utmpos midpos = newpos(oldend, 90 + tmpAngle, minLaneSeparationINMeters);
                        
                        if (midpos.GetDistance(oldend) < midpos.GetDistance(newstart) &&
                            midpos.GetDistance(newstart) > newstart.GetDistance(oldend))
                        {
                        }
                        else
                        {
                            tmpAngle += 180;
                        }
                       
                        midpos = newpos(oldend, 90 + tmpAngle, minLaneSeparationINMeters);                      
                        ans.Add(midpos);
                        midpos = newpos(midpos, angle, sign * minLaneSeparationINMeters);
                        ans.Add(midpos);
                        midpos = newpos(midpos, 270 + tmpAngle, minLaneSeparationINMeters);
                        ans.Add(midpos);
                    }
                } else {
                    double cos_phi = cos_aob(startposutm, newstart, newend);
                    if (cos_phi > 0)
                    {
                        if (Math.Pow(startposutm.GetDistance(newstart), 2) * (1 - cos_phi * cos_phi) > 4 * Math.Pow(minLaneSeparationINMeters, 2))
                        {
                            utmpos midpos = newpos(newstart, 90 + angle, minLaneSeparationINMeters);
                            if (midpos.GetDistance(startposutm) > startposutm.GetDistance(newstart))
                            {
                                midpos = newpos(newstart, 270 + angle, minLaneSeparationINMeters);
                            }
                            ans.Add(midpos);
                        }
                        else
                        {
                            utmpos midpos = newpos(newstart, 90 + angle, minLaneSeparationINMeters);
                            double tmpAngle = angle;
                            if (midpos.GetDistance(startposutm) < startposutm.GetDistance(newstart))
                            {
                                tmpAngle += 180;
                            }
                            int sign = flightForward ? -1 : 1;
                            midpos = newpos(newstart, 90 + tmpAngle, minLaneSeparationINMeters);
                            ans.Add(midpos);
                            midpos = newpos(midpos, angle, sign * minLaneSeparationINMeters);
                            ans.Add(midpos);
                            midpos = newpos(midpos, 270 + tmpAngle, minLaneSeparationINMeters);
                            ans.Add(midpos);
                        }
                    }

                }
                flightForward = !flightForward;
                i++;
            }    

            addtomap(newstart, "S");
            ans.Add(newstart);
            for (int j = 0; j < tmp.Count; j++)
                ans.Add(tmp[j]);
            tmp.Clear();
            addtomap(newend, "E");
            ans.Add(newend);

                

            // set the altitude on all points
            ans.ForEach(plla => { plla.Alt = altitude; });

            return ans;
        }