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 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]]); }
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); }
//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); }