public PointLatLngAlt getMousePos(int x, int y) { //https://gamedev.stackexchange.com/questions/103483/opentk-ray-picking int[] viewport = new int[4]; Matrix4 modelMatrix, projMatrix; MakeCurrent(); GL.GetFloat(GetPName.ModelviewMatrix, out modelMatrix); GL.GetFloat(GetPName.ProjectionMatrix, out projMatrix); GL.GetInteger(GetPName.Viewport, viewport); var _start = UnProject(new Vector3(x, y, 0.0f), projMatrix, modelMatrix, new Size(viewport[2], viewport[3])); var _end = UnProject(new Vector3(x, y, 1), projMatrix, modelMatrix, new Size(viewport[2], viewport[3])); var pos = new utmpos(utmcenter[0] + _end.X, utmcenter[1] + _end.Y, utmzone); var plla = pos.ToLLA(); plla.Alt = _end.Z; var point = srtm.getIntersectionWithTerrain(center, plla); return(point); }
/// <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); }
/// <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)); }
/// <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(); }
private static List <utmpos> GenerateOffsetPath(List <utmpos> utmpositions, double distance, int utmzone) { List <utmpos> ans = new List <utmpos>(); utmpos oldpos = utmpos.Zero; for (int a = 0; a < utmpositions.Count - 2; a++) { var prevCenter = utmpositions[a]; var currCenter = utmpositions[a + 1]; var nextCenter = utmpositions[a + 2]; var l1bearing = prevCenter.GetBearing(currCenter); var l2bearing = currCenter.GetBearing(nextCenter); var l1prev = Newpos(prevCenter, l1bearing + 90, distance); var l1curr = Newpos(currCenter, l1bearing + 90, distance); var l2curr = Newpos(currCenter, l2bearing + 90, distance); var l2next = Newpos(nextCenter, l2bearing + 90, distance); var l1l2center = FindLineIntersectionExtension(l1prev, l1curr, l2curr, l2next); //start if (a == 0) { // add start l1prev.Tag = "S"; ans.Add(l1prev); // add start/trigger l1prev.Tag = "SM"; ans.Add(l1prev); oldpos = l1prev; } //middle of leg l1l2center.Tag = "M"; ans.Add(l1l2center); oldpos = l1l2center; // last leg if ((a + 3) == utmpositions.Count) { l2next.Tag = "ME"; ans.Add(l2next); l2next.Tag = "E"; ans.Add(l2next); } } return(ans); }
/// <summary> /// this is a debug function /// </summary> /// <param name="pos"></param> /// <param name="tag"></param> static void addtomap(utmpos pos, string tag) { return; //if (tag == "M") // return; //polygons.Markers.Add(new GMapMarkerWP(pos.ToLLA(), tag)); //map.ZoomAndCenterMarkers("polygons"); //map.Invalidate(); }
// 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)); }
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); }
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); }
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]]); }
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) { //DoDebug(); if (spacing < 4 && spacing != 0) { spacing = 4; } 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); } // pick start positon based on initial point rectangle 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; case StartPosition.Point: startposutm = new utmpos(StartPointLatLngAlt); break; } // find the closes polygon point based from our startpos selection startposutm = findClosestPoint(startposutm, utmpositions); // 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; } // S = start // E = end // ME = middle end // SM = start middle 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); newstart.Tag = "S"; addtomap(newstart, "S"); ans.Add(newstart); closest.p1.Tag = "SM"; addtomap(closest.p1, "SM"); ans.Add(closest.p1); 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); var utmpos1 = new utmpos(ax, ay, utmzone) { Tag = "M" }; addtomap(utmpos1, "M"); ans.Add(utmpos1); } } closest.p2.Tag = "ME"; addtomap(closest.p2, "ME"); ans.Add(closest.p2); utmpos newend = newpos(closest.p2, angle, overshoot1); newend.Tag = "E"; 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); newstart.Tag = "S"; addtomap(newstart, "S"); ans.Add(newstart); closest.p2.Tag = "SM"; addtomap(closest.p2, "SM"); ans.Add(closest.p2); 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); var utmpos2 = new utmpos(ax, ay, utmzone) { Tag = "M" }; addtomap(utmpos2, "M"); ans.Add(utmpos2); } } closest.p1.Tag = "ME"; addtomap(closest.p1, "ME"); ans.Add(closest.p1); utmpos newend = newpos(closest.p1, angle, -overshoot2); newend.Tag = "E"; 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); }
private static List <utmpos> GenerateOffsetPath(List <utmpos> utmpositions, double distance, double spacing, int utmzone) { List <utmpos> ans = new List <utmpos>(); utmpos oldpos = utmpos.Zero; for (int a = 0; a < utmpositions.Count - 2; a++) { var prevCenter = utmpositions[a]; var currCenter = utmpositions[a + 1]; var nextCenter = utmpositions[a + 2]; var l1bearing = prevCenter.GetBearing(currCenter); var l2bearing = currCenter.GetBearing(nextCenter); var l1prev = newpos(prevCenter, l1bearing + 90, distance); var l1curr = newpos(currCenter, l1bearing + 90, distance); var l2curr = newpos(currCenter, l2bearing + 90, distance); var l2next = newpos(nextCenter, l2bearing + 90, distance); var l1l2center = FindLineIntersectionExtension(l1prev, l1curr, l2curr, l2next); //start if (a == 0) { // add start l1prev.Tag = "S"; ans.Add(l1prev); // add start/trigger l1prev.Tag = "SM"; ans.Add(l1prev); oldpos = l1prev; } //spacing if (spacing > 0) { for (int d = (int)((oldpos.GetDistance(l1l2center)) % spacing); d < (oldpos.GetDistance(l1l2center)); d += (int)spacing) { double ax = oldpos.x; double ay = oldpos.y; newpos(ref ax, ref ay, l1bearing, d); var utmpos2 = new utmpos(ax, ay, utmzone) { Tag = "M" }; ans.Add(utmpos2); } } //end of leg l1l2center.Tag = "S"; ans.Add(l1l2center); oldpos = l1l2center; // last leg if ((a + 3) == utmpositions.Count) { if (spacing > 0) { for (int d = (int)((l1l2center.GetDistance(l2next)) % spacing); d < (l1l2center.GetDistance(l2next)); d += (int)spacing) { double ax = l1l2center.x; double ay = l1l2center.y; newpos(ref ax, ref ay, l2bearing, d); var utmpos2 = new utmpos(ax, ay, utmzone) { Tag = "M" }; ans.Add(utmpos2); } } l2next.Tag = "ME"; ans.Add(l2next); l2next.Tag = "E"; ans.Add(l2next); } } return(ans); }
/// <summary> /// this is a debug function /// </summary> /// <param name="pos"></param> /// <param name="tag"></param> static void Addtomap(utmpos pos, string tag) { polygons.Markers.Add(new GMapMarkerWP(pos.ToLLA(), tag)); }
public static List <PointLatLngAlt> CreateGrid(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter) { if (spacing < 10 && spacing != 0) { spacing = 10; } 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(GridPlugin.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 linelatlng closest = findClosestLine(startposutm, grid); utmpos lastpnt; if (closest.p1.GetDistance(startposutm) < closest.p2.GetDistance(startposutm)) { lastpnt = closest.p1; } else { lastpnt = closest.p2; } while (grid.Count > 0) { if (closest.p1.GetDistance(lastpnt) < closest.p2.GetDistance(lastpnt)) { addtomap(closest.p1, "S"); ans.Add(closest.p1); 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" })); // if (shutter.ToLower().StartsWith("y")) // AddDigicamControlPhoto(); } } utmpos newend = newpos(closest.p2, angle, overshoot1); // if (overshoot1 > 0) // ans.Add(new utmpos(closest.p2) { Tag = "M" }); addtomap(newend, "E"); ans.Add(newend); lastpnt = closest.p2; grid.Remove(closest); if (grid.Count == 0) { break; } closest = findClosestLine(newend, grid); } else { addtomap(closest.p2, "S"); ans.Add(closest.p2); 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" })); // if (shutter.ToLower().StartsWith("y")) // AddDigicamControlPhoto(); } } 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); } } // set the altitude on all points ans.ForEach(plla => { plla.Alt = altitude; }); return(ans); }
//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); }