コード例 #1
0
        //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);
        }