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]]);
}
/// <summary>
/// 任务分段
/// </summary>
/// <param name="points">任务航点</param>
/// <param name="distance">每一段任务长度</param>
/// <returns></returns>
public List <utmpos> LineCut(List <utmpos> points, double distance)
{
// 1. 航点坐标转墨卡托
// 2. 任务分段
// 3. 墨卡托转经纬度
// 4. 写数据库
// 结果
List <utmpos> result = new List <utmpos>();
// 创建一个堆栈
Stack <utmpos> stacks = new Stack <utmpos>();
for (int i = points.Count - 1; i >= 0; i--)
{
stacks.Push(points[i]);
}
//
double total = 0;
utmpos frompoint = stacks.Pop();
result.Add(frompoint); // 添加第一个节点
for (; stacks.Count > 0;)
{
utmpos topoint = stacks.Pop();
//
double len = frompoint.GetDistance(topoint);
if (len + total < distance)
{
// 还没有达到极限
result.Add(topoint); // 加到结果
frompoint = topoint; // 记录最后一个点
total = total + len; // 线段累加
continue;
}
else if (len + total > distance)
{
// 剩下长度不够达到下一个节点
double x = 0;
double y = 0;
double dis = distance - total;
GetCutPoint(frompoint.x, frompoint.y, topoint.x, topoint.y, dis, out x, out y); // 计算中间节点
utmpos cutpoint = new utmpos(x, y, frompoint.zone); // 中间节点
result.Add(cutpoint); // 加到结果
result.Add(new utmpos(0, 0, 0)); // 添加分隔 ##
frompoint = cutpoint; // 起始点定义到中间节点
result.Add(frompoint); // 添加起始点
stacks.Push(topoint); // 结束点重新压回堆栈
total = 0; // 总长度归零
continue;
}
else
{
// 刚刚好
result.Add(topoint); // 添加结果
if (stacks.Count > 0)
{
// 如果后面还有数据就继续处理
result.Add(new utmpos(0, 0, 0)); // 添加分隔 ##
frompoint = topoint; // 记录最后一个点
result.Add(frompoint); // 结果添加新的起始点
total = 0; // 总长度归零
}
continue;
}
}
return(result);
}