/// <summary> /// from http://stackoverflow.com/questions/1119451/how-to-tell-if-a-line-intersects-a-polygon-in-c /// </summary> /// <param name="start1"></param> /// <param name="end1"></param> /// <param name="start2"></param> /// <param name="end2"></param> /// <returns></returns> public static utmpos FindLineIntersection(utmpos start1, utmpos end1, utmpos start2, utmpos end2) { double denom = ((end1.x - start1.x) * (end2.y - start2.y)) - ((end1.y - start1.y) * (end2.x - start2.x)); // AB & CD are parallel if (denom == 0) { return(utmpos.Zero); } double numer = ((start1.y - start2.y) * (end2.x - start2.x)) - ((start1.x - start2.x) * (end2.y - start2.y)); double r = numer / denom; double numer2 = ((start1.y - start2.y) * (end1.x - start1.x)) - ((start1.x - start2.x) * (end1.y - start1.y)); double s = numer2 / denom; if ((r < 0 || r > 1) || (s < 0 || s > 1)) { return(utmpos.Zero); } // Find intersection point utmpos result = new utmpos(); result.x = start1.x + (r * (end1.x - start1.x)); result.y = start1.y + (r * (end1.y - start1.y)); result.zone = start1.zone; return(result); }
public utmpos(utmpos pos) { this.x = pos.x; this.y = pos.y; this.zone = pos.zone; this.Tag = null; }
// polar to rectangular static utmpos newpos(utmpos input, double bearing, double distance) { double degN = 90 - bearing; if (degN < 0) { degN += 360; } double x = input.x + distance * Math.Cos(degN * deg2rad); double y = input.y + distance * Math.Sin(degN * deg2rad); return(new utmpos(x, y, input.zone)); }
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 bool PointInPolygon(utmpos p, List <utmpos> poly) { utmpos p1, p2; bool inside = false; if (poly.Count < 3) { return(inside); } utmpos oldPoint = new utmpos(poly[poly.Count - 1]); for (int i = 0; i < poly.Count; i++) { utmpos newPoint = new utmpos(poly[i]); if (newPoint.y > oldPoint.y) { p1 = oldPoint; p2 = newPoint; } else { p1 = newPoint; p2 = oldPoint; } if ((newPoint.y < p.y) == (p.y <= oldPoint.y) && ((double)p.x - (double)p1.x) * (double)(p2.y - p1.y) < ((double)p2.x - (double)p1.x) * (double)(p.y - p1.y)) { inside = !inside; } oldPoint = newPoint; } return(inside); }
///// <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); }
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); }
public double GetDistance(utmpos b) { return(Math.Sqrt(Math.Pow(Math.Abs(x - b.x), 2) + Math.Pow(Math.Abs(y - b.y), 2))); }