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