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