C# Utilities.PointLatLngAlt类代码示例

        void SendGrid(double lat, double lon, ushort grid_spacing, byte bit)
        {
            log.Info("SendGrid "+ lat + " " + lon + " space " + grid_spacing + " bit "+ bit);

            MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t();
            resp.grid_spacing = grid_spacing;
            resp.lat = lastrequest.lat;
            resp.lon = lastrequest.lon;
            resp.gridbit = bit;
            resp.data = new short[16];

            for (int i = 0; i < (4 * 4); i++)
            {
                int y = i % 4;
                int x = i / 4;

                PointLatLngAlt plla = new PointLatLngAlt(lat, lon).location_offset(x * grid_spacing, y * grid_spacing);

                double alt = srtm.getAltitude(plla.Lat, plla.Lng);

                resp.data[i] = (short)alt;
            }

            MainV2.comPort.sendPacket(resp);
        }

        public List<PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2)
        {
            List<PointLatLngAlt> answer = new List<PointLatLngAlt>();

            spline_t = spline_t_sq = 0.0f;

            spline_p0 = currentpos;
            spline_p1 = p1;
            spline_p2 = p2;
            spline_p0_prime = new Vector3();
            double yaw = (currentpos.GetBearing(p1) + 360) % 360;
            spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad);
            spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad);
            spline_p0_prime.z = spline_p1.z - spline_p0.z;

            initialize_spline_segment();

            int steps = 30;

            foreach (var step in range(steps +1))
            {
                spline_t = (1f / steps) * step;
                spline_t_sq = spline_t * spline_t;
                evaluate_spline();
                answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, ""));
            }

            return answer;
        }

        public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint)
        {
            log.Info("getAirports " + centerpoint);

            // check if we have moved 66% from our last cache center point
            if (currentcenter.GetDistance(centerpoint) < ((proximity/3)*2))
            {
                 return cache;
            }

            log.Info("getAirports - regen list");

            // generate a new list
            currentcenter = centerpoint;

            cache.Clear();

            foreach (PointLatLngAlt item in airports)
            {
                if (item.GetDistance(centerpoint) < proximity)
                {
                    cache.Add(item);
                }
            }

            return cache;
        }

 public utmpos(PointLatLngAlt pos)
 {
     double[] dd = pos.ToUTM(pos.GetUTMZone());
     this.x = dd[0];
     this.y = dd[1];
     this.zone = pos.GetUTMZone();
     this.Tag = null;
 }

 public PointLatLngAlt(PointLatLngAlt plla)
 {
     this.Lat = plla.Lat;
     this.Lng = plla.Lng;
     this.Alt = plla.Alt;
     this.color = plla.color;
     this.Tag = plla.Tag;
 }

        public override void Update()
        {
            if (MainV2.comPort.MAV.cs.lat == 0 || MainV2.comPort.MAV.cs.lng == 0)
                return;
            if (Leader == null)
                Leader = MainV2.comPort;

            masterpos = new PointLatLngAlt(Leader.MAV.cs.lat, Leader.MAV.cs.lng, Leader.MAV.cs.alt, "");
        }

        public override void SendCommand()
        {
            if (masterpos.Lat == 0 || masterpos.Lng == 0)
                return;

            Console.WriteLine(DateTime.Now);
            Console.WriteLine("Leader {0} {1} {2}",masterpos.Lat,masterpos.Lng,masterpos.Alt);

            int a = 0;
            foreach (var port in MainV2.Comports)
            {
                if (port == Leader)
                    continue;

                PointLatLngAlt target = new PointLatLngAlt(masterpos);

                try
                {
                    //convert Wgs84ConversionInfo to utm
                    CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();

                    GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;

                    int utmzone = (int)((masterpos.Lng - -186.0) / 6.0);

                    IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true);

                    ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);

                    double[] pll1 = { target.Lng, target.Lat };

                    double[] p1 = trans.MathTransform.Transform(pll1);

                    // add offsets to utm
                    p1[0] += ((HIL.Vector3)offsets[port]).x;
                    p1[1] += ((HIL.Vector3)offsets[port]).y;

                    // convert back to wgs84
                    IMathTransform inversedTransform = trans.MathTransform.Inverse();
                    double[] point = inversedTransform.Transform(p1);

                    target.Lat = point[1];
                    target.Lng = point[0];
                    target.Alt += ((HIL.Vector3)offsets[port]).z;

                    port.setGuidedModeWP(new Locationwp() { alt = (float)target.Alt, lat = target.Lat, lng = target.Lng, id = (byte)MAVLink.MAV_CMD.WAYPOINT });

                    Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);

                }
                catch (Exception ex) { Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString()); }

                a++;
            }


        }

        // pv_latlon_to_vector - convert lat/lon coordinates to a position vector
        public Vector3 pv_location_to_vector(PointLatLngAlt loc)
        {
            PointLatLngAlt home = MainV2.comPort.MAV.cs.HomeLocation;

            scaleLongDown = longitude_scale(home);
            scaleLongUp = 1.0f / scaleLongDown;

            Vector3 tmp = new Vector3((loc.Lat * 1.0e7f - home.Lat * 1.0e7f) * LATLON_TO_CM, (loc.Lng * 1.0e7f - home.Lng * 1.0e7f) * LATLON_TO_CM * scaleLongDown, loc.Alt * 100);
            return tmp;
        }

        public static PointLatLngAlt ProjectPoint()
        {
            PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);

            double yawangle = MainV2.comPort.MAV.cs.campointc;
            double rollangle = MainV2.comPort.MAV.cs.campointb;
            double pitchangle = MainV2.comPort.MAV.cs.campointa;

            if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
            {
                yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
                rollangle = MainV2.comPort.MAVlist[71].cs.roll;
                pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
            }

            int distout = 10;
            PointLatLngAlt newpos = PointLatLngAlt.Zero;

            //dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);

            while (distout < 1000)
            {
                // get a projected point to test intersection against - not using slope distance
                PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
                newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;

                // get another point 50 infront
                PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
                newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;

                // get the flat terrain distance out - at 0 alt
                double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);

                // x is dist from plane, y is alt
                var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
                    new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));

                if (newpoint.X != 0)
                {
                    newpos = newposdist2;
                    break;
                }

                distout += 50;
            }

            //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));

            //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);

            //Console.WriteLine(newpos);
            return newpos;
        }

        public static void AddAirport(PointLatLngAlt plla)
        {
            // foreach (PointLatLngAlt item in airports)
            {
              //  if (item.GetDistance(plla) < 200)
                {
              //      return;
                }
            }

            airports.Add(plla);
        }

        public static void POIAdd(PointLatLngAlt Point, string tag)
        {
            // local copy
            PointLatLngAlt pnt = Point;

            pnt.Tag = tag + "\n" + pnt.ToString();

            POI.POIs.Add(pnt);

            if (POIModified != null)
                POIModified(null, null);
        }

 public static void POIDelete(PointLatLngAlt Point)
 {
     for (int a = 0; a < POI.POIs.Count; a++)
     {
         if (POI.POIs[a].Point() == Point)
         {
             POI.POIs.RemoveAt(a);
             if (POIModified != null)
                 POIModified(null, null);
             return;
         }
     }
 }

        public PointLatLngAlt ToLLA()
        {
            IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(Math.Abs(zone), zone < 0 ? false : true);

            ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);

            // get leader utm coords
            double[] pll = trans.MathTransform.Inverse().Transform(this);

            PointLatLngAlt ans = new PointLatLngAlt(pll[1], pll[0]);
            if (this.Tag != null)
                ans.Tag = this.Tag.ToString();

            return ans;
        }

        public static List<PointLatLngAlt> GeneratePath(MAVState MAV)
        {
            List<PointLatLngAlt> result = new List<PointLatLngAlt>();

            MAVLink.mavlink_mission_item_t? prevwp = null;

            int a = -1;

            foreach (var wp in MAV.wps.Values)
            {
                a++;
                if (!prevwp.HasValue)
                {
                    // change firstwp/aka home to valid alt
                    prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0});
                    continue;
                }

                if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
                    continue;

                var wp1 = new PointLatLngAlt(prevwp.Value);
                var wp2 = new PointLatLngAlt(wp);
                var bearing = wp1.GetBearing(wp2);
                var distwps = wp1.GetDistance(wp2);
                var startalt = wp1.Alt;
                var endalt = wp2.Alt;

                if (startalt == 0)
                {
                    startalt = endalt;
                }

                if(distwps > 5000)
                    continue;

                for (double d = 0.1; d < distwps; d += 0.1)
                {
                    var pnt = wp1.newpos(bearing, d);
                    pnt.Alt = startalt + (d/distwps) * (endalt-startalt);
                    result.Add(pnt);
                }

                prevwp = wp;
            }

            return result;
        }

        PointLatLngAlt FindTrailPnt(PointLatLngAlt from)
        {
            // get the start point for the distance
            int start = trail.IndexOf(from);

            for (int i = start+1; i < trail.Count; i++)
            {
                double dist = from.GetDistance(trail[i]); // 2d distance
                if (dist > FollowDistance)
                {
                    return trail[i];
                }
            }

            return null;
        }