//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 * deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * 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) * 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); } GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); 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 * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * 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 * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * deg2rad, 0); * mz.from_euler(0, 0, Y * 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) * 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); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }