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");
}
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)
});
}
static private 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);
}
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();
}
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();
}
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);
}
}
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);
}
static private linelatlng FindLowestFscore(Hashtable f_score, List <linelatlng> openset)
{
linelatlng lowest = openset[0];
int lowestint = int.MaxValue;
foreach (linelatlng key in openset)
{
if (f_score.ContainsKey(key) && (double)f_score[key] < lowestint)
{
lowestint = (int)(double)f_score[key];
lowest = key;
}
}
Console.WriteLine("Lowest " + lowestint);
return(lowest);
}
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);
}
///// <summary>
/////
///// </summary>
///// <param name="pos"></param>
//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");
//}
///// <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));
// map.ZoomAndCenterMarkers("polygons");
// map.Invalidate();
//}
//static GMapOverlay polygons = new GMapOverlay("polygons");
//static GMapControl map = new GMapControl();
/// <summary>
/// 关键函数
/// </summary>
/// <param name="polygon">测区范围</param>
/// <param name="altitude">飞行高度</param>
/// <param name="distance">航线间距</param>
/// <param name="spacing">拍照间隔</param>
/// <param name="angle">航线角度</param>
/// <param name="overshoot1">向往扩展1</param>
/// <param name="overshoot2">向外扩张2</param>
/// <param name="startpos">起始航电</param>
/// <param name="shutter">电子快门</param>
/// <param name="minLaneSeparation">最小??</param>
/// <param name="leadin">引导线</param>
/// <returns></returns>
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;
// 区域中点XY
// 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);
startposutm = new utmpos(area.Left, area.Bottom, utmzone);
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 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);
}
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;
}
//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;
}
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;
}
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;
}
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));
}
static void addtomap(linelatlng pos)
{
}
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);
}
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;
}
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;
}
}
//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);
}
