static void addtomap(PointLatLngAlt pos, string tag) { }
public string SUBMITHOST; // Submitter Host “1.2.3.4” or “enduser5.faa.gov” public List <List <PointLatLng> > GetPaths() { //RLN27.576944W97.108611LN27.468056W96.961111LN27.322222W97.050000LN27.345833W97.088889LN27.439167W97.186944RLN27.672778W97.212222LN27.576944W97.108611LN27.533333W97.133333LN27.638333W97.237222RCN27.686333W97.294667R007.00 List <List <PointLatLng> > list = new List <List <PointLatLng> >(); List <PointLatLng> pointlist = new List <PointLatLng>(); var matches = all.Matches(BOUND); bool isarcterminate = false; bool iscircleterminate = false; int arcdir = 0; PointLatLngAlt pointcent = null; PointLatLngAlt pointstart = null; foreach (Match item in matches) { try { if (item.Groups[0].Value.ToString().StartsWith("R") || item.Groups[0].Value.ToString().StartsWith("B")) { // start new element if (pointlist.Count > 0) { list.Add(pointlist); pointlist = new List <PointLatLng>(); } } if (item.Groups[2].Value == "L") { var point = new PointLatLngAlt(double.Parse(item.Groups[4].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[6].Value, CultureInfo.InvariantCulture)); if (item.Groups[3].Value == "S") { point.Lat *= -1; } if (item.Groups[5].Value == "W") { point.Lng *= -1; } if (isarcterminate) { double radius = pointcent.GetDistance(pointstart); double startbearing = pointcent.GetBearing(pointstart); double endbearing = pointcent.GetBearing(point); if (arcdir > 0 && endbearing < startbearing) { endbearing += 360; } if (arcdir < 0) { for (double a = startbearing; a > endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } else { for (double a = startbearing; a < endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } pointlist.Add(point); isarcterminate = false; iscircleterminate = false; continue; } if (iscircleterminate) { iscircleterminate = false; continue; } pointlist.Add(point); continue; } else if (item.Groups[7].Value == "A") { pointcent = new PointLatLngAlt(double.Parse(item.Groups[10].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[12].Value, CultureInfo.InvariantCulture)); if (item.Groups[9].Value == "S") { pointcent.Lat *= -1; } if (item.Groups[11].Value == "W") { pointcent.Lng *= -1; } pointstart = new PointLatLngAlt(double.Parse(item.Groups[14].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[16].Value, CultureInfo.InvariantCulture)); if (item.Groups[13].Value == "S") { pointstart.Lat *= -1; } if (item.Groups[15].Value == "W") { pointstart.Lng *= -1; } arcdir = item.Groups[8].Value == "+" ? 1 : -1; isarcterminate = true; continue; } else if (item.Groups[17].Value == "C") { var point = new PointLatLngAlt(double.Parse(item.Groups[19].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[21].Value, CultureInfo.InvariantCulture)); if (item.Groups[18].Value == "S") { point.Lat *= -1; } if (item.Groups[20].Value == "W") { point.Lng *= -1; } // radius in m from nautical miles double radius = double.Parse(item.Groups[22].Value, CultureInfo.InvariantCulture) * 1852; for (int a = 0; a <= 360; a += 10) { pointlist.Add(point.newpos(a, radius)); } list.Add(pointlist); pointlist = new List <PointLatLng>(); iscircleterminate = true; continue; } } catch { } } if (pointlist.Count > 0) { list.Add(pointlist); } return(list); }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll // if roll and pitch is 0, use the quick method. if (R == 0 && P == 0) { // calc fov in m on the ground at 0 alt var fovh = Math.Tan(hfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovd = Math.Sqrt(fovh * fovh + fovv * fovv); // where we put our footprint var ans2 = new List <PointLatLngAlt>(); // calc bearing from center to corner var bearing1 = Math.Atan2(fovh, fovv) * MathHelper.rad2deg; // calc first corner point var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv)); // set alt to 0, as we used the hypot for distance and fov is based on 0 alt newpos1.Alt = 0; // calc intersection from center to new point and return ground hit point var cen1 = calcIntersection(plla, newpos1); // add to our footprint poly ans2.Add(cen1); addtomap(cen1, "cen1"); //repeat newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen2"); newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen3"); newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen4"); addtomap(plla, "plane"); return(ans2); } double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * MathHelper.deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * MathHelper.deg2rad, 0); * mz.from_euler(0, 0, Y * MathHelper.deg2rad); * * printdcm(mx); * printdcm(my); * printdcm(mz); * printdcm(my * mx); * printdcm(mz * my * mx); * * test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * MathHelper.rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); addtomap(groundpointbr, "br"); // var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }