public utmpos(PointLatLngAlt pos) { double[] dd = pos.ToUTM(); this.x = dd[0]; this.y = dd[1]; this.zone = pos.GetUTMZone(); this.Tag = null; }
public static List <PointLatLngAlt> CreateCorridor(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, double width, float leadin = 0) { if (spacing < 4 && spacing != 0) { spacing = 4; } if (distance < 0.1) { distance = 0.1; } if (polygon.Count == 0) { return(new List <PointLatLngAlt>()); } 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); var lanes = (width / distance) + 2; var start = (int)((lanes / 2) * -1); var end = start * -1; for (int lane = start; lane <= end; lane++) { // correct side of the line we are on because of list reversal int multi = 1; if ((lane - start) % 2 == 1) { multi = -1; } GenerateOffsetPath(utmpositions, distance * multi * lane, spacing, utmzone) .ForEach(pnt => { ans.Add(pnt); }); utmpositions.Reverse(); } // set the altitude on all points ans.ForEach(plla => { plla.Alt = altitude; }); return(ans); }
double[] convertCoords(PointLatLngAlt plla) { if (utmzone < -360) { utmzone = plla.GetUTMZone(); } var utm = plla.ToUTM(utmzone); Array.Resize(ref utm, 3); utm[2] = plla.Alt; return(utm); }
double[] convertCoords(PointLatLngAlt plla) { if (utmzone != plla.GetUTMZone()) { utmzone = plla.GetUTMZone(); utmcenter = plla.ToUTM(utmzone); textureid.ForEach(a => a.Value.Cleanup()); textureid.Clear(); } var utm = plla.ToUTM(utmzone); Array.Resize(ref utm, 3); utm[0] -= utmcenter[0]; utm[1] -= utmcenter[1]; utm[2] = plla.Alt; return(new[] { utm[0], utm[1], utm[2] }); }
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) { //DoDebug(); if (spacing < 4 && spacing != 0) { spacing = 4; } 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); } // pick start positon based on initial point rectangle 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; case StartPosition.Point: startposutm = new utmpos(StartPointLatLngAlt); break; } // find the closes polygon point based from our startpos selection startposutm = findClosestPoint(startposutm, utmpositions); // 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> CreateCorridor(List <PointLatLngAlt> polygon, double height, double camViewHeight, double camVertSpacing, double distance, double angle, double camPitch, bool flipDirection, double bermDepth, int numBenches, double toeHeight, double toepoint, double toepoint_runs, bool pathHome, double homeAlt, FlightPlanner.altmode altmode) { int direction = (flipDirection == true ? -1 : 1); if (camVertSpacing < 0.1) { camVertSpacing = 0.1; } if (polygon.Count == 0 || numBenches < 1) { return(new List <PointLatLngAlt>()); } 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); double vertOffset = 0; double horizOffset = 0; double toepoint_runs_count = 0; //calculate number of lanes with a starting altitude of half the calculated cam view height // double initialAltitude = camViewHeight * Math.Sin(angle * deg2rad) / 3; double initialAltitude = toepoint; var vertIncrement = camVertSpacing * Math.Sin(angle * deg2rad); var lanes = Math.Round((height - initialAltitude) / vertIncrement) + toepoint_runs + 1; //repeat for each bench, applying height/berm depth offsets for (int bench = 0; bench < numBenches; bench++) { //repeat for each increment up face for (int lane = 0; lane < lanes; lane++) { if (toepoint_runs_count < toepoint_runs) { //calculate offset from the base of the face based on toe angle, camera pitch, camera overlap % and bench offset vertOffset = distance * Math.Sin(camPitch * deg2rad) + (initialAltitude + (bench * height) + toeHeight); horizOffset = distance * Math.Cos(camPitch * deg2rad) - ((initialAltitude) / Math.Tan(angle * deg2rad)) - bench * (bermDepth + height / Math.Tan(angle * deg2rad)); toepoint_runs_count++; } else { //calculate offset from the base of the face based on toe angle, camera pitch, camera overlap % and bench offset vertOffset = distance * Math.Sin(camPitch * deg2rad) + (initialAltitude + ((lane - toepoint_runs) * vertIncrement) + (bench * height) + toeHeight); horizOffset = distance * Math.Cos(camPitch * deg2rad) - ((initialAltitude + ((lane - toepoint_runs) * vertIncrement)) / Math.Tan(angle * deg2rad)) - bench * (bermDepth + height / Math.Tan(angle * deg2rad)); } // THIS IS COMMENTED OUT BECAUSE IT'S INSANE NONSENSE THAT SHOULD NEVER HAVE BEEN IN HERE. // // //convert to absolute if flight planner is set to absolute mode (shift up by home alt) // if (altmode == FlightPlanner.altmode.Absolute) // { // vertOffset += homeAlt; // } //if this is the first lane of a bench, climb to the altitude of the first waypoint of the lane before moving to the waypoint if (lane == 0 && ans.Count > 0) { PointLatLngAlt intermediateWP = new PointLatLngAlt(ans.Last().Lat, ans.Last().Lng, vertOffset) { Tag = "S" }; ans.Add(intermediateWP); } GenerateOffsetPath(utmpositions, horizOffset * direction, utmzone) .ForEach(pnt => { ans.Add(pnt); ans.Last().Alt = vertOffset; }); //reverse the order of waypoints and direction of offset on the way back utmpositions.Reverse(); direction = -direction; } } //if an odd number of lanes were specified create one last run along the path back to home if (pathHome && ((lanes * numBenches) % 2) == 1) { GenerateOffsetPath(utmpositions, horizOffset * direction, utmzone) .ForEach(pnt => { ans.Add(pnt); ans.Last().Alt = vertOffset; ans.Last().Tag = "R"; }); } 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) { 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); }