C# PointLatLngAlt.GetDistance方法代码示例

        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;
        }

//.........这里部分代码省略.........
                            string[] fields2 = item.name.Split(' ');

                            // check primare firmware type. ie arudplane, arducopter
                            if (fields1[0] == fields2[0])
                            {
                                Version ver1 = VersionDetection.GetVersion(comPort.MAV.VersionString);
                                Version ver2 = VersionDetection.GetVersion(item.name);

                                if (ver2 > ver1)
                                {
                                    Common.MessageShowAgain(Strings.NewFirmware, Strings.NewFirmwareA + item.name + Strings.Pleaseup);
                                    break;
                                }

                                // check the first hit only
                                break;
                            }
                        }
                    }
                    catch (Exception ex) { log.Error(ex); }
                }

                FlightData.CheckBatteryShow();

                MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Connect", comPort.MAV.cs.firmware.ToString(), comPort.MAV.param.Count.ToString());
                MissionPlanner.Utilities.Tracking.AddTiming("Connect", "Connect Time", (DateTime.Now - connecttime).TotalMilliseconds, "");

                MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Baud", comPort.BaseStream.BaudRate.ToString(), "");

                // save the baudrate for this port
                config[_connectionControl.CMB_serialport.Text + "_BAUD"] = _connectionControl.CMB_baudrate.Text;

                this.Text = titlebar + " " + comPort.MAV.VersionString;

                // refresh config window if needed
                if (MyView.current != null)
                {
                    if (MyView.current.Name == "HWConfig")
                        MyView.ShowScreen("HWConfig");
                    if (MyView.current.Name == "SWConfig")
                        MyView.ShowScreen("SWConfig");
                }

                // load wps on connect option.
                if (config["loadwpsonconnect"] != null && bool.Parse(config["loadwpsonconnect"].ToString()) == true)
                {
                    // only do it if we are connected.
                    if (comPort.BaseStream.IsOpen)
                    {
                        MenuFlightPlanner_Click(null, null);
                        FlightPlanner.BUT_read_Click(null, null);
                    }
                }

                // get any rallypoints
                if (MainV2.comPort.MAV.param.ContainsKey("RALLY_TOTAL") && int.Parse(MainV2.comPort.MAV.param["RALLY_TOTAL"].ToString()) > 0)
                {
                    FlightPlanner.getRallyPointsToolStripMenuItem_Click(null, null);

                    double maxdist = 0;

                    foreach (var rally in comPort.MAV.rallypoints)
                    {
                        foreach (var rally1 in comPort.MAV.rallypoints)
                        {
                            var pnt1 = new PointLatLngAlt(rally.Value.lat / 10000000.0f, rally.Value.lng / 10000000.0f);
                            var pnt2 = new PointLatLngAlt(rally1.Value.lat / 10000000.0f, rally1.Value.lng / 10000000.0f);

                            var dist = pnt1.GetDistance(pnt2);

                            maxdist = Math.Max(maxdist, dist);
                        }
                    }

                    if (comPort.MAV.param.ContainsKey("RALLY_LIMIT_KM") && (maxdist / 1000.0) > (float)comPort.MAV.param["RALLY_LIMIT_KM"])
                    {
                        CustomMessageBox.Show(Strings.Warningrallypointdistance + " " + (maxdist / 1000.0).ToString("0.00") + " > " + (float)comPort.MAV.param["RALLY_LIMIT_KM"]);
                    }
                }

                // set connected icon
                this.MenuConnect.Image = displayicons.disconnect;
            }
            catch (Exception ex)
            {
                log.Warn(ex);
                try
                {
                    _connectionControl.IsConnected(false);
                    UpdateConnectIcon();
                    comPort.Close();
                }
                catch (Exception ex2) 
                {
                    log.Warn(ex2);
                }
                CustomMessageBox.Show("Can not establish a connection\n\n" + ex.Message);
                return;
            }
        }

        //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;

//.........这里部分代码省略.........

        static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100)
        {
            int distout = 10;
            PointLatLngAlt newpos = PointLatLngAlt.Zero;

            var dist = plla.GetDistance(dest);
            var Y = plla.GetBearing(dest);

            // 20 km
            while (distout < (dist+100))
            {
                // get a projected point to test intersection against - not using slope distance
                PointLatLngAlt newposdist = plla.newpos(Y, distout);
                newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;

                // get another point 'step' infront
                PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step);
                newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;

                // x is dist from plane, y is alt
                var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt),
                    new PointF((float)dist, (float)dest.Alt),
                    new PointF((float)distout, (float)newposdist.Alt),
                    new PointF((float)distout + step, (float)newposdist2.Alt));

                if (newpoint.X != 0)
                {
                    newpos = plla.newpos(Y, newpoint.X);
                    newpos.Alt = newpoint.Y;

                    return newpos;
                }

                distout += step;
            }

            //addtomap(newpos, distout.ToString());

            dest.Alt = 0;

            return dest;
        }

//.........这里部分代码省略.........
                        //sp._flags.reached_destination = true;
                        //sp._origin = sp.pv_location_to_vector(lastpnt);
                        //sp._destination = sp.pv_location_to_vector(fullpointlist[0]);

                        // sp._spline_origin_vel = sp.pv_location_to_vector(lastpnt) - sp.pv_location_to_vector(lastnonspline);

                        sp.set_wp_origin_and_destination(sp.pv_location_to_vector(lastpnt2), sp.pv_location_to_vector(lastpnt));

                        sp._flags.reached_destination = true;

                        for (int no = 1; no < (splinepnts.Count - 1); no++)
                        {
                            Spline2.spline_segment_end_type segtype = Spline2.spline_segment_end_type.SEGMENT_END_STRAIGHT;

                            if (no < (splinepnts.Count - 2))
                            {
                                segtype = Spline2.spline_segment_end_type.SEGMENT_END_SPLINE;
                            }

                            sp.set_spline_destination(sp.pv_location_to_vector(splinepnts[no]), false, segtype, sp.pv_location_to_vector(splinepnts[no + 1]));

                            //sp.update_spline();

                            while (sp._flags.reached_destination == false)
                            {
                                float t = 1f;
                                //sp.update_spline();
                                sp.advance_spline_target_along_track(t);
                                // Console.WriteLine(sp.pv_vector_to_location(sp.target_pos).ToString());
                                list.Add(sp.pv_vector_to_location(sp.target_pos));
                            }

                            list.Add(splinepnts[no]);

                        }

                        list.ForEach(x =>
                        {
                            wproute.Add(x);
                        });


                        splinepnts.Clear();

                        /*
                        MissionPlanner.Controls.Waypoints.Spline sp = new Controls.Waypoints.Spline();
                        
                        var spline = sp.doit(splinepnts, 20, lastlastpnt.GetBearing(splinepnts[0]),false);

                  
                         */

                        lastnonspline = fullpointlist[a];
                    }

                    wproute.Add(fullpointlist[a]);

                    lastpnt2 = lastpnt;
                    lastpnt = fullpointlist[a];
                }
            }
            /*

           List<PointLatLng> list = new List<PointLatLng>();
           fullpointlist.ForEach(x => { list.Add(x); });
           route.Points.AddRange(list);
           */
            // route is full need to get 1, 2 and last point as "HOME" route

            int count = wproute.Count;
            int counter = 0;
            PointLatLngAlt homepoint = new PointLatLngAlt();
            PointLatLngAlt firstpoint = new PointLatLngAlt();
            PointLatLngAlt lastpoint = new PointLatLngAlt();

            if (count > 2)
            {
                // homeroute = last, home, first
                wproute.ForEach(x =>
                {
                    counter++;
                    if (counter == 1) { homepoint = x; return; }
                    if (counter == 2) { firstpoint = x; }
                    if (counter == count - 1) { lastpoint = x; }
                    if (counter == count) { homeroute.Points.Add(lastpoint); homeroute.Points.Add(homepoint); homeroute.Points.Add(firstpoint); return; }
                    route.Points.Add(x);
                });

                homeroute.Stroke = new Pen(Color.Yellow, 2);
                // if we have a large distance between home and the first/last point, it hangs on the draw of a the dashed line.
                if (homepoint.GetDistance(lastpoint) < 5000 && homepoint.GetDistance(firstpoint) < 5000)
                    homeroute.Stroke.DashStyle = DashStyle.Dash;

                polygonsoverlay.Routes.Add(homeroute);

                route.Stroke = new Pen(Color.Yellow, 4);
                route.Stroke.DashStyle = DashStyle.Custom;
                polygonsoverlay.Routes.Add(route);
            }
        }

        //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();
//.........这里部分代码省略.........

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

            for (int i = start; i > 0; i--)
            {
                double dist = from.GetDistance(trail[i]); // 2d distance
                if (dist > Seperation)
                {
                    return trail[i];
                }
            }

            return null;
        }

        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);

                    double heading = -Leader.MAV.cs.yaw;

                    double length = offsets[port].length();

                    var x = ((HIL.Vector3)offsets[port]).x;
                    var y = ((HIL.Vector3)offsets[port]).y;

                    // add offsets to utm
                    p1[0] += x*Math.Cos(heading*deg2rad) - y*Math.Sin(heading*deg2rad);
                    p1[1] += x*Math.Sin(heading*deg2rad) + y*Math.Cos(heading*deg2rad);

                    if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
                    {
                        // project the point forwards gs*5
                        var gs = port.MAV.cs.groundspeed;

                        p1[1] += gs*5*Math.Cos((-heading)*deg2rad);
                        p1[0] += gs*5*Math.Sin((-heading)*deg2rad);
                    }
                    // 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;

                    if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
                    {
                        var dist = target.GetDistance(new PointLatLngAlt(port.MAV.cs.lat, port.MAV.cs.lng, port.MAV.cs.alt));

                        dist -= port.MAV.cs.groundspeed*5;

                        var leadergs = Leader.MAV.cs.groundspeed;

                        var newspeed = (leadergs + (float) (dist/10));

                        if (newspeed < 5)
                            newspeed = 5;

                        port.setParam("TRIM_ARSPD_CM", newspeed*100.0f);
                    }

                    port.setGuidedModeWP(new Locationwp()
                    {
                        alt = (float) target.Alt,
                        lat = target.Lat,
                        lng = target.Lng,
                        id = (ushort)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++;
            }
        }

        private List<PointLatLngAlt> GetLocations(List<PointLatLngAlt> path, PointLatLngAlt location, double lead, double seperation)
        {
            List<PointLatLngAlt> result = new List<PointLatLngAlt>();

            // get the current location closest point
            PointLatLngAlt closestPointLatLngAlt = PointLatLngAlt.Zero;
            double mindist = 99999999;
            foreach (var pointLatLngAlt in path)
            {
                var distloc = location.GetDistance(pointLatLngAlt);
                if (distloc < mindist)
                {
                    mindist = distloc;
                    closestPointLatLngAlt = pointLatLngAlt;
                }
            }

            var start = path.IndexOf(closestPointLatLngAlt);
            var a = 0;
            for (int i = start; i < (path.Count - 1); i++)
            {
                var targetdistance = lead - a * seperation;

                if (targetdistance < 0)
                    i-=2;

                if (i < 0)
                    break;

                double dist = closestPointLatLngAlt.GetDistance(path[i]); // 2d distance
                if (dist >= Math.Abs(targetdistance))
                {
                    result.Add(path[i]);
                    i = start;

                    if (result.Count > 20)
                        break;
                    if (seperation == 0)
                        break;
                    a++;
                }
            }

            return result;
        }