//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 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; var fovh = Math.Tan(hfov/2.0 * deg2rad)*2.0; var fovv = Math.Tan(vfov/2.0 * deg2rad)*2.0; //DoDebug(); 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(); 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; }
//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; }