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, 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; }
static linelatlng findClosestLine(utmpos start, List <linelatlng> list, double minDistance, double angle) { if (minDistance == 0) { 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); } // By now, just add 5.000 km to our lines so they are long enough to allow intersection double METERS_TO_EXTEND = 5000; 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) { // Calculate intersection point utmpos p = FindLineIntersectionExtension(line.p1, line.p2, 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]; } var filteredlist = intersectedPoints.Where(a => a.Key.GetDistance(start) >= key); return(findClosestLine(start, filteredlist.Select(a => a.Value).ToList(), 0, angle)); }
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); }
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 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>(); 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; }
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); }