private void buildCircPath()
        {
            if (!_build)
            {
                return;
            }

            // Get Parameters

            double lat_home          = _lat;
            double lon_home          = _lon;
            double lat_center        = _lat;
            double lon_center        = _lon;
            double altitude          = Convert.ToDouble(txtDiaAddCircPathAlt.Text);
            double circle_radius     = Convert.ToDouble(txtDiaAddCircPathRadius.Text);
            double start_angle       = Convert.ToDouble(txtCircStartAngle.Text);
            double circle_span       = Convert.ToDouble(txtCircSpan.Text);
            int    circle_num_points = Convert.ToInt16(txtCircNumPoints.Text);
            bool   startend          = chkCircHome.Checked;

            int[,] no_actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };
            int[,] actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };

            // Get Camera Location

            double lat_camera   = lat_center;
            double lon_camera   = lon_center;
            String cam_poi_name = cmbCircPOI.GetItemText(cmbCircPOI.SelectedItem);

            for (int i = 0; i < _wpg.POICount(); i++)
            {
                POIPoints tmp_point = _wpg.POIPointAt(i);
                string    name      = tmp_point.name;
                if (cam_poi_name == name)
                {
                    lat_camera = tmp_point.lat;
                    lon_camera = tmp_point.lon;
                }
            }

            // Generate Full 360 degree circle

            LinkedList <WayPoints> new_list = new LinkedList <WayPoints>();
            int    gimblemode  = 0;
            double gimplepitch = -GPS.RadiansToDegrees(Math.Atan(altitude / circle_radius));
            double curvesize   = 0;
            double rotdir      = 0;

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, 0.0, curvesize, rotdir, 0, 0.0, no_actions);
            }
            int    count   = 0;
            double heading = 0;
            double distance;
            double angle           = start_angle;
            double angle_increment = circle_span / (circle_num_points);
            bool   full_circle     = chkFullCircle.Checked;

            if (circle_span == 360.0)
            {
                full_circle = true;
            }
            double circ_lat;
            double circ_lon;
            double start_lat  = 0;
            double start_lon  = 0;
            double start_head = 0;

            if (full_circle)
            {
                angle_increment = 360.0 / circle_num_points;
            }
            if (radioCCW.Checked)
            {
                angle_increment = -angle_increment;
            }
            int num_points = circle_num_points;

            if (!full_circle)
            {
                num_points++;
            }
            do
            {
                circ_lat = GPS.GPS_Lat_BearDist(lat_center, lon_center, angle, circle_radius, Form1.Globals.gps_radius);
                circ_lon = GPS.GPS_Lon_BearDist(lat_center, lon_center, circ_lat, angle, circle_radius, Form1.Globals.gps_radius);

                if (chkCircPOI.Checked)
                {
                    heading     = GPS.GPS_Bearing(circ_lat, circ_lon, lat_camera, lon_camera);
                    distance    = GPS.GPS_Distance(circ_lat, circ_lon, lat_camera, lon_camera, Form1.Globals.gps_radius);
                    gimplepitch = -GPS.RadiansToDegrees(Math.Atan(altitude / distance));
                    gimblemode  = 2;
                }
                else
                {
                    heading = GPS.Mod_Angle(angle + 90);
                    if (radioCCW.Checked)
                    {
                        heading = GPS.Mod_Angle(heading + 180.0);
                    }
                    gimplepitch = 0;
                    gimblemode  = 0;
                }

                if (count == 0)
                {
                    start_lat = circ_lat; start_lon = circ_lon; start_head = heading;
                }

                _wp.Add_Waypoint_List(new_list, circ_lat, circ_lon, altitude, heading, curvesize, rotdir, gimblemode, gimplepitch, actions);

                angle = angle + angle_increment;
                count++;
            } while (count < num_points);

            if (full_circle)
            {
                _wp.Add_Waypoint_List(new_list, start_lat, start_lon, altitude, start_head, curvesize, rotdir, gimblemode, gimplepitch, actions);
            }
            ;

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, 0.0, curvesize, rotdir, gimblemode, 0.0, no_actions);
            }

            // Save Path

            if (_new_path & _first_pass)
            {
                string path_name = txtDiaAddCircPathName.Text;
                if (path_name == "")
                {
                    path_name = "Untitled - Circular";
                }
                _path.Add_Path(_wpg, _gmap, path_name, "Circular", new_list);
                _path       = _wpg.PathAt(_wpg.PathCount() - 1);
                _first_pass = false;
                //Models.Path newpath = _wpg.PathAt(_current_path_index);
                //newpath.visible = true;
                //newpath.selected = true;
            }
            else
            {
                _path.waypoints = new_list;
                //_wpg.ChangePathWP(_path, new_list);
                //Models.Path path = _wpg.PathAt(_current_path_index);
                //_gmap.Delete_gMapPath(path);
                //_gmap.Add_gMapPath(path, false);
            }
            _path.visible  = true;
            _path.selected = false;
            //_gmap.BuildgMap();
            _gmap.ReDrawgMap();
        }
        private void BuildRectPath()
        {
            if (!_build)
            {
                return;
            }

            // Get center position

            double lat_home   = _lat;
            double lon_home   = _lon;
            double lat_center = _lat;
            double lon_center = _lon;
            string path_name  = txtRectPathName.Text;

/*
 *          int poi_count = _wpg.POICount();
 *          POIPoints tmp_point;
 *          for (int i = 0; i < poi_count; i++)
 *          {
 *              tmp_point = _wpg.POIPointAt(i);
 *              string name = tmp_point.name;
 *              if (home_name == name)
 *              {
 *
 *                  lat_home = tmp_point.lat;
 *                  lon_home = tmp_point.lon;
 *              }
 *          }
 */
            double lat_camera;
            double lon_camera;

            LinkedList <WayPoints> new_list = new LinkedList <WayPoints>();

            // Get Values from input Boxes

            _video = radioVideo.Checked;
            bool OnePass   = chkOnePass.Checked;
            bool startend  = chkRectHome.Checked;
            bool camerapoi = chkRectCamPOI.Checked;
            int  poi_count;

            if (camerapoi)
            {
                string poi_name = cmbRectCamPOI.GetItemText(cmbRectCamPOI.SelectedItem);
                poi_count = _wpg.POICount();

                for (int i = 0; i < poi_count; i++)
                {
                    POIPoints tmp_point = _wpg.POIPointAt(i);
                    string    name      = tmp_point.name;
                    if (poi_name == name)
                    {
                        lat_camera = tmp_point.lat;
                        lon_camera = tmp_point.lon;
                    }
                }
            }

            double box_width  = Convert.ToDouble(txtGridLength.Text);
            double box_height = Convert.ToDouble(txtGridWidth.Text);
            double altitude   = Convert.ToDouble(txtPathAlt.Text);

            _camera_width  = (2 * (Math.Tan(GPS.DegreesToRadians(_camera_angle_hor / 2)) * altitude));
            _camera_height = (2 * (Math.Tan(GPS.DegreesToRadians(_camera_angle_ver / 2)) * altitude));

            if (!Form1.Globals.UnitsMetric)
            {
                box_width      = FeetToMeters(box_width);
                box_height     = FeetToMeters(box_height);
                _camera_width  = FeetToMeters(_camera_width);
                _camera_height = FeetToMeters(_camera_height);
            }
            //double box_rotate = Convert.ToDouble(txtGridRotation.Text);
            double box_rotate = Convert.ToDouble(txtGridRotation.Text) - 90.0;
            //double box_rotate = 0.0;

            double hor_overlap = _overlap_width / 100;
            double ver_overlap = _overlap_height / 100;



            double gps_radius = Form1.Globals.gps_radius;

            int[,] actions = new int[, ] {
                { 0, 2000 }, { 1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };
            int[,] no_actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };

            /* Start Calculating */


            double diagonal_length = Math.Sqrt((box_width / 2) * (box_width / 2) + (box_height / 2) * (box_height / 2));
            double diagonal_angle  = GPS.RadiansToDegrees(Math.Atan((box_width / 2) / (box_height / 2)));



            /* Calculate Top, Bottom, Left & Right Center Positions */

            double lat_top;
            double lon_top;
            double lat_bottom;
            double lon_bottom;
            double lat_left;
            double lon_left;
            double lat_right;
            double lon_right;

            lat_top = GPS.GPS_Lat_BearDist(lat_center, lon_center, 0, box_height / 2, gps_radius);
            lon_top = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, 0, box_height / 2, gps_radius);

            lat_bottom = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180, box_height / 2, gps_radius);
            lon_bottom = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_bottom, 180, box_height / 2, gps_radius);

            lat_left = GPS.GPS_Lat_BearDist(lat_center, lon_center, 270, box_width / 2, gps_radius);
            lon_left = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_left, 270, box_width / 2, gps_radius);

            lat_right = GPS.GPS_Lat_BearDist(lat_center, lon_center, 90, box_width / 2, gps_radius);
            lon_right = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_right, 90, box_width / 2, gps_radius);

            /* Calculate 4 corner locations */

            double lat_top_left;
            double lon_top_left;
            double lat_bottom_left;
            double lon_bottom_left;
            double lat_top_right;
            double lon_top_right;
            double lat_bottom_right;
            double lon_bottom_right;

            double[,] polypnt = new double[4, 2];

            lat_top_left  = GPS.GPS_Lat_BearDist(lat_center, lon_center, -diagonal_angle, diagonal_length, gps_radius);
            lon_top_left  = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, -diagonal_angle, diagonal_length, gps_radius);
            polypnt[0, 0] = lat_top_left;
            polypnt[0, 1] = lon_top_left;

            lat_top_right = GPS.GPS_Lat_BearDist(lat_center, lon_center, diagonal_angle, diagonal_length, gps_radius);
            lon_top_right = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, diagonal_angle, diagonal_length, gps_radius);
            polypnt[1, 0] = lat_top_right;
            polypnt[1, 1] = lon_top_right;

            lat_bottom_right = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180 - diagonal_angle, diagonal_length, gps_radius);
            lon_bottom_right = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, 180 - diagonal_angle, diagonal_length, gps_radius);
            polypnt[2, 0]    = lat_bottom_right;
            polypnt[2, 1]    = lon_bottom_right;

            lat_bottom_left = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180 + diagonal_angle, diagonal_length, gps_radius);
            lon_bottom_left = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, 180 + diagonal_angle, diagonal_length, gps_radius);
            polypnt[3, 0]   = lat_bottom_left;
            polypnt[3, 1]   = lon_bottom_left;

            // Rotate Points

            double new_lat, new_lon;


            for (int i = 0; i < 4; i++)
            {
                double rot_bearing  = GPS.GPS_Bearing(lat_center, lon_center, polypnt[i, 0], polypnt[i, 1]);
                double rot_distance = GPS.GPS_Distance(lat_center, lon_center, polypnt[i, 0], polypnt[i, 1], gps_radius);

                // Calculate a new location by increasing the bearing

                new_lat       = GPS.GPS_Lat_BearDist(lat_center, lon_center, rot_bearing + box_rotate, rot_distance, gps_radius);
                new_lon       = GPS.GPS_Lon_BearDist(lat_center, lon_center, new_lat, rot_bearing + box_rotate, rot_distance, gps_radius);
                polypnt[i, 0] = new_lat;
                polypnt[i, 1] = new_lon;
            }

            // Generate Rectangle Polygon


            _poly.name    = "temp";
            _poly.visible = true;
            LinkedList <PolyPoint> shape_points = new LinkedList <PolyPoint>();

            for (int i = 0; i <= 3; i++)
            {
                PolyPoint newpnt = new PolyPoint();
                newpnt.lat = polypnt[i, 0];
                newpnt.lon = polypnt[i, 1];
                newpnt.alt = 10;
                shape_points.AddLast(newpnt);
            }
            PolyPoint pnt = new PolyPoint();

            pnt.lat = polypnt[0, 0];
            pnt.lon = polypnt[0, 1];
            pnt.alt = 10;
            shape_points.AddLast(pnt);
            _poly.points = shape_points;

            // Get Direction Flags

            double drone_heading = 90.0;
            int    ver_dir_flag  = 1;

            if (radioUpLeft.Checked)
            {
                ver_dir_flag = 1;
            }
            if (radioUpRight.Checked)
            {
                ver_dir_flag = 1; drone_heading = 270;
            }
            if (radioLowLeft.Checked)
            {
                ver_dir_flag = -1;
            }
            if (radioLowRight.Checked)
            {
                ver_dir_flag = -1; drone_heading = 270;
            }

            // Grid Calculations

            double lat_next;
            double next_lon_start;
            double next_lon_end;
            double lon_tmp;
            double row_increment = _camera_width * (1.0 - hor_overlap);
            int    num_rows      = Convert.ToInt16(box_height / row_increment);

            box_rotate = 0.0;

            // Create Waypoints

            int    gimblemode  = 2;
            double gimblepitch = -90.0;
            double curvesize   = 0;
            double rotdir      = 0;

            if (OnePass | num_rows == 1)
            {
                if (startend)
                {
                    _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                }

                if (radioUpLeft.Checked | radioLowLeft.Checked)
                {
                    _wp.Add_Leg_List(new_list, lat_left, lon_left, lat_right, lon_right,
                                     altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                }
                else
                {
                    _wp.Add_Leg_List(new_list, lat_right, lon_right, lat_left, lon_left,
                                     altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                }

                if (startend)
                {
                    _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                }
            }
            else
            {
                /* More than one row */

                if (num_rows == 2)
                {
                    double offset            = row_increment;
                    double diagonal_length_2 = Math.Sqrt((box_width / 2) * (box_width / 2) + (offset / 2) * (offset / 2));
                    double diagonal_angle_2  = GPS.RadiansToDegrees(Math.Atan((box_width / 2) / (offset / 2)));

                    lat_top_left = GPS.GPS_Lat_BearDist(lat_center, lon_center, -diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);
                    lon_top_left = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, -diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);

                    lat_top_right = GPS.GPS_Lat_BearDist(lat_center, lon_center, diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);
                    lon_top_right = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);

                    lat_bottom_left = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180 + diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);
                    lon_bottom_left = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, 180 + diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);

                    lat_bottom_right = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180 - diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);
                    lon_bottom_right = GPS.GPS_Lon_BearDist(lat_center, lon_center, lat_top, 180 - diagonal_angle_2 + box_rotate, diagonal_length_2, gps_radius);

                    if (startend)
                    {
                        _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                    }
                    if (radioLowLeft.Checked)
                    {
                        _wp.Add_Leg_List(new_list, lat_bottom_left, lon_bottom_left, lat_bottom_right, lon_bottom_right,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                        _wp.Add_Leg_List(new_list, lat_top_right, lon_top_right, lat_top_left, lon_top_left,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                    }

                    if (radioLowRight.Checked)
                    {
                        _wp.Add_Leg_List(new_list, lat_bottom_right, lon_bottom_right, lat_bottom_left, lon_bottom_left,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                        _wp.Add_Leg_List(new_list, lat_top_left, lon_top_left, lat_top_right, lon_top_right,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                    }

                    if (radioUpLeft.Checked)
                    {
                        _wp.Add_Leg_List(new_list, lat_top_left, lon_top_left, lat_top_right, lon_top_right,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                        _wp.Add_Leg_List(new_list, lat_bottom_right, lon_bottom_right, lat_bottom_left, lon_bottom_left,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                    }

                    if (radioUpRight.Checked)
                    {
                        _wp.Add_Leg_List(new_list, lat_top_right, lon_top_right, lat_top_left, lon_top_left,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                        _wp.Add_Leg_List(new_list, lat_bottom_left, lon_bottom_left, lat_bottom_right, lon_bottom_right,
                                         altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                    }

                    if (startend)
                    {
                        _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                    }
                }
                else
                {
                    /* More than 2 rows */

                    double row_span = (num_rows - 1) * row_increment;
                    lat_top    = GPS.GPS_Lat_BearDist(lat_center, lon_center, 0 + box_rotate, row_span / 2, gps_radius);
                    lat_bottom = GPS.GPS_Lat_BearDist(lat_center, lon_center, 180 + box_rotate, row_span / 2, gps_radius);
                    double degrees_per_foot = (lat_top - lat_bottom) / row_span;

                    if (startend)
                    {
                        _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                    }

                    int row_count = 0;
                    next_lon_start = 0.0;
                    next_lon_end   = 0.0;
                    double lat_current = 0.0;

                    do
                    {
                        /* First Row */

                        if (row_count == 0)
                        {
                            if (radioLowLeft.Checked)
                            {
                                _wp.Add_Leg_List(new_list, lat_bottom, lon_left, lat_bottom, lon_right,
                                                 altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                                lat_current    = lat_bottom;
                                next_lon_start = lon_right;
                                next_lon_end   = lon_left;
                            }
                            if (radioLowRight.Checked)
                            {
                                _wp.Add_Leg_List(new_list, lat_bottom, lon_right, lat_bottom, lon_left,
                                                 altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                                lat_current    = lat_bottom;
                                next_lon_start = lon_left;
                                next_lon_end   = lon_right;
                            }
                            if (radioUpLeft.Checked)
                            {
                                _wp.Add_Leg_List(new_list, lat_top, lon_left, lat_top, lon_right,
                                                 altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                                lat_current    = lat_top;
                                next_lon_start = lon_right;
                                next_lon_end   = lon_left;
                            }
                            if (radioUpRight.Checked)
                            {
                                _wp.Add_Leg_List(new_list, lat_top, lon_right, lat_top, lon_left,
                                                 altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                                lat_current    = lat_top;
                                next_lon_start = lon_left;
                                next_lon_end   = lon_right;
                            }
                        }
                        else
                        {
                            /* Output 2nd, 3rd, etc. */

                            //lat_current = Globals.waypoints[Globals.waypoint_count - 1, 0];
                            lat_next = lat_current + (-ver_dir_flag * (degrees_per_foot * row_increment));
                            _wp.Add_Leg_List(new_list, lat_next, next_lon_start, lat_next, next_lon_end,
                                             altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, actions, _video, _camera_height, _overlap_height);
                            lon_tmp        = next_lon_start;
                            next_lon_start = next_lon_end;
                            next_lon_end   = lon_tmp;
                            lat_current    = lat_next;
                        }
                        row_count++;
                    } while (row_count < num_rows);

                    if (startend)
                    {
                        _wp.Add_Waypoint_List(new_list, lat_home, lon_home, altitude, drone_heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                    }
                }
            }


            // Rotate WayPoints about Center

            int    count = 0;
            double lat, lon, alt, head, bearing, distance;
            double lat_first = lat_center;
            double lon_first = lon_center;
            LinkedListNode <WayPoints> node = new_list.First;
            //LinkedListNode<WayPoints> next_node = node.Next;
            LinkedListNode <WayPoints> save_node;

            //lat_first = node.Value.lat;
            //lon_first = node.Value.lon;
            //alt_first = node.Value.alt;
            //head_first = node.Value.head;

            while (node != null)
            {
                if (count != 0)
                {
                    // Get Waypoint values

                    lat         = node.Value.lat;
                    lon         = node.Value.lon;
                    alt         = node.Value.alt;
                    head        = node.Value.head;
                    gimblemode  = node.Value.gimblemode;
                    gimblepitch = node.Value.gimblepitch;
                    curvesize   = node.Value.curvesize;
                    rotdir      = node.Value.rotationdir;
                    actions     = node.Value.actions;
                    box_rotate  = Convert.ToDouble(txtGridRotation.Text) - 90.0;

                    // Calculate Bearing and distance from first waypoint

                    bearing  = GPS.GPS_Bearing(lat_first, lon_first, lat, lon);
                    distance = GPS.GPS_Distance(lat_first, lon_first, lat, lon, gps_radius);

                    // Calculate a new location by increasing the bearing

                    new_lat = GPS.GPS_Lat_BearDist(lat_first, lon_first, bearing + box_rotate, distance, gps_radius);
                    new_lon = GPS.GPS_Lon_BearDist(lat_first, lon_first, new_lat, bearing + box_rotate, distance, gps_radius);

                    // Set new waypoint

                    WayPoints new_wp = new WayPoints();
                    new_wp.lat         = new_lat;
                    new_wp.lon         = new_lon;
                    new_wp.alt         = alt;
                    new_wp.head        = head + box_rotate;
                    new_wp.gimblemode  = gimblemode;
                    new_wp.gimblepitch = gimblepitch;
                    new_wp.curvesize   = curvesize;
                    new_wp.rotationdir = rotdir;
                    new_wp.actions     = actions;
                    save_node          = node;
                    new_list.AddBefore(node, new_wp);
                    node = node.Next;
                    new_list.Remove(save_node);
                }
                count++;
            }

            // Redo start and end points

            if (startend)
            {
                WayPoints wp = new_list.First.Value;
                new_list.RemoveFirst();
                wp.lat = lat_home;
                wp.lon = lon_home;
                new_list.AddFirst(wp);
                wp = new_list.Last.Value;
                new_list.RemoveLast();
                wp.lat = lat_home;
                wp.lon = lon_home;
                new_list.AddLast(wp);
            }

            // Save Path

            if (_new_path & _first_pass)
            {
                path_name = txtRectPathName.Text;
                if (path_name == "")
                {
                    path_name = "Untitled - Rectangular";
                }
                _path.Add_Path(_wpg, _gmap, path_name, "Rectangular", new_list);
                Path newpath = _wpg.PathAt(_wpg.PathCount() - 1);
                _current_intid = newpath.internal_id;
                _first_pass    = false;
            }
            else
            {
                _wpg.ChangePathWPIntId(_current_intid, new_list);
                //Models.Path path = _wpg.PathAt(_current_path_index);
                //_gmap.Delete_gMapPath(path);
                //_gmap.Add_gMapPath(path, false);
            }

            _gmap.ReDrawgMap();
            _gmap.Add_gMapPoly(_poly, true);
        }
        private void btnOutputKML_Click(object sender, EventArgs e)
        {
            LinkedList <WayPoints> wp_list = _path.waypoints;

            int pcount      = 0;
            int point_count = wp_list.Count;

            if (point_count > 0)
            {
                //double lat_center = Convert.ToDouble(txtCenterLat.Text);
                //double lon_center = Convert.ToDouble(txtCenterLon.Text);
                double lat, new_lat, start_lat;
                double lon, new_lon, start_lon;
                double image_len;
                double image_wid;

                image_len = (Math.Tan(GPS.DegreesToRadians(_options.focal_angle_hor / 2)) * 30.0);
                image_wid = (Math.Tan(GPS.DegreesToRadians(_options.focal_angle_ver / 2)) * 30.0);
                double alt;
                double bear;
                string location_name = txtKMLPath.Text;
                string filename      = txtKMLFilePath.Text;

                Kml kml = new Kml();

                Folder folder = new Folder();
                SharpKml.Dom.Placemark placemark  = new SharpKml.Dom.Placemark();
                LineString             linestring = new LineString();
                var vector = new Vector();
                CoordinateCollection coordinates = new CoordinateCollection();

                Style path_style = new Style();
                path_style.Polygon = new PolygonStyle();
                path_style.Line    = new LineStyle();
                // Color32( alpha, blue, green, red
                path_style.Polygon.Color = new Color32(128, 255, 255, 0);
                path_style.Line.Color    = new Color32(255, 255, 255, 0);
                linestring.AltitudeMode  = AltitudeMode.RelativeToGround;
                linestring.Extrude       = true;
                WayPoints point = new WayPoints();

                do
                {
                    point = wp_list.ElementAt(pcount);
                    lat   = point.lat;
                    lon   = point.lon;
                    alt   = point.alt;
                    if (!_options.units_metric)
                    {
                        alt = GPS.FeetToMeters(alt);
                    }

                    vector = new Vector(lat, lon, alt);

                    coordinates.Add(vector);
                    pcount++;
                } while (pcount < point_count);

                linestring.Coordinates = coordinates;
                placemark.Geometry     = linestring;
                placemark.AddStyle(path_style);
                placemark.Name = location_name;
                folder.AddFeature(placemark);
                // kml.Feature = placemark;

                /* Generate camera rectangles */

                if (chkGenCamRect.Checked)
                {
                    //image_len = 25;
                    //image_wid = 50;
                    double diag_ang = GPS.RadiansToDegrees(Math.Atan(image_len / image_wid));
                    double diag_len = Math.Sqrt((image_len * image_len) + (image_wid * image_wid));
                    double gps_radius;

                    for (int i = 0; i < wp_list.Count; i++)
                    {
                        Style poly_style = new Style();
                        poly_style.Polygon = new PolygonStyle();
                        poly_style.Line    = new LineStyle();
                        // Color32( alpha, blue, green, red
                        poly_style.Polygon.Color = new Color32(128, 0, 255, 255);
                        poly_style.Line.Color    = new Color32(255, 0, 255, 255);
                        Kml kml_rect = new Kml();
                        SharpKml.Dom.Placemark rect_placemark = new SharpKml.Dom.Placemark();
                        Polygon              poly             = new Polygon();
                        OuterBoundary        outer            = new OuterBoundary();
                        LinearRing           line             = new LinearRing();
                        CoordinateCollection coor             = new CoordinateCollection();
                        poly.AltitudeMode = AltitudeMode.RelativeToGround;
                        poly.Extrude      = false;

                        point      = wp_list.ElementAt(i);
                        lat        = point.lat;
                        lon        = point.lon;
                        alt        = point.alt;
                        gps_radius = _options.earth_radius + _options.def_elevation + alt;
                        alt        = 2;
                        bear       = point.head;
                        coor.Clear();
                        new_lat   = GPS.GPS_Lat_BearDist(lat, lon, diag_ang + bear, diag_len, gps_radius);
                        new_lon   = GPS.GPS_Lon_BearDist(lat, lon, new_lat, diag_ang + bear, diag_len, gps_radius);
                        start_lat = new_lat;
                        start_lon = new_lon;
                        vector    = new Vector(new_lat, new_lon, alt);
                        coor.Add(vector);
                        new_lat = GPS.GPS_Lat_BearDist(lat, lon, 180 - diag_ang + bear, diag_len, gps_radius);
                        new_lon = GPS.GPS_Lon_BearDist(lat, lon, new_lat, 180 - diag_ang + bear, diag_len, gps_radius);
                        vector  = new Vector(new_lat, new_lon, alt);
                        coor.Add(vector);
                        new_lat = GPS.GPS_Lat_BearDist(lat, lon, 180 + diag_ang + bear, diag_len, gps_radius);
                        new_lon = GPS.GPS_Lon_BearDist(lat, lon, new_lat, 180 + diag_ang + bear, diag_len, gps_radius);
                        vector  = new Vector(new_lat, new_lon, alt);
                        coor.Add(vector);
                        new_lat = GPS.GPS_Lat_BearDist(lat, lon, 360 - diag_ang + bear, diag_len, gps_radius);
                        new_lon = GPS.GPS_Lon_BearDist(lat, lon, new_lat, 360 - diag_ang + bear, diag_len, gps_radius);
                        vector  = new Vector(new_lat, new_lon, alt);
                        coor.Add(vector);
                        vector = new Vector(start_lat, start_lon, alt);
                        coor.Add(vector);

                        line.Coordinates   = coor;
                        outer.LinearRing   = line;
                        poly.OuterBoundary = outer;

                        rect_placemark.Geometry = poly;
                        rect_placemark.AddStyle(poly_style);
                        rect_placemark.Name = "WP " + Convert.ToString(i);
                        folder.AddFeature(rect_placemark);
                    }
                }

                kml.Feature = folder;
                Serializer serializer = new Serializer();
                serializer.Serialize(kml);


                System.IO.File.WriteAllText(filename, serializer.Xml);
            }
            this.Close();
        }
Exemplo n.º 4
0
        private void buildHelicalPath()
        {
            if (!_build)
            {
                return;
            }

            bool   startend           = chkHelicalHome.Checked;
            double lat_home           = _lat;
            double lon_home           = _lon;
            double lat_center         = _lat;
            double lon_center         = _lon;
            double helix_start_alt    = Convert.ToDouble(txtHelixStartAlt.Text);
            double helix_end_alt      = Convert.ToDouble(txtHelixEndAlt.Text);
            double helix_start_radius = Convert.ToDouble(txtHelixStartRadius.Text);
            double helix_end_radius   = Convert.ToDouble(txtHelixEndRadius.Text);
            double helix_start_angle  = Convert.ToDouble(txtHelixStartAngle.Text);
            double helix_span         = Convert.ToDouble(txtHelixSpan.Text);
            int    helix_num_points   = Convert.ToInt16(txtHelixNumPoints.Text);

            int[,] no_actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };
            int[,] actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };

            double cw = 1.0;

            if (radioCCW.Checked)
            {
                cw = -1.0;
            }

            double lat_camera   = lat_center;
            double lon_camera   = lon_center;
            String cam_poi_name = cmbHelixPOI.GetItemText(cmbHelixPOI.SelectedItem);

            for (int i = 0; i < _wpg.POICount(); i++)
            {
                POIPoints tmp_point = _wpg.POIPointAt(i);
                string    name      = tmp_point.name;
                if (cam_poi_name == name)
                {
                    lat_camera = tmp_point.lat;
                    lon_camera = tmp_point.lon;
                }
            }

            LinkedList <WayPoints> new_list = new LinkedList <WayPoints>();
            int    count           = 0;
            double angle           = helix_start_angle;
            double angle_increment = cw * helix_span / helix_num_points;
            double circ_lat;
            double circ_lon;
            //if (chkFullCircle.Checked) angle_increment = 360.0 / helix_num_points;
            //if (radioCCW.Checked) angle_increment = -angle_increment;
            double alt_diff = helix_end_alt - helix_start_alt;
            int    alt_dir;

            if (helix_start_alt > helix_end_alt)
            {
                alt_dir = -1;
            }
            else
            {
                alt_dir = 1;
            }
            double alt_increment    = (helix_end_alt - helix_start_alt) / helix_num_points;
            double radius_increment = (helix_end_radius - helix_start_radius) / helix_num_points;
            double helix_radius;
            double helix_altitude;
            int    gimblemode  = 0;
            double gimplepitch = 0;
            double distance;
            double curvesize = 0;
            double rotdir    = 0;

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, lat_home, lon_home, helix_start_alt, 0.0, curvesize, rotdir, gimblemode, gimplepitch, no_actions);
            }
            do
            {
                double heading = GPS.Mod_Angle(angle + 180.0);
                helix_radius   = helix_start_radius + (count * radius_increment);
                helix_altitude = helix_start_alt + (count * alt_increment);
                //norm_angle = 180.0*(angle / (helix_end_angle - helix_start_angle));
                //helix_altitude = helix_start_alt + (alt_diff * alt_dir * (1+Math.Cos(DegreesToRadians(norm_angle)))/2);
                circ_lat = GPS.GPS_Lat_BearDist(lat_center, lon_center, angle, helix_radius, Form1.Globals.gps_radius);
                circ_lon = GPS.GPS_Lon_BearDist(lat_center, lon_center, circ_lat, angle, helix_radius, Form1.Globals.gps_radius);

                if (chkHelixPOI.Checked)
                {
                    heading     = GPS.GPS_Bearing(circ_lat, circ_lon, lat_camera, lon_camera);
                    distance    = GPS.GPS_Distance(circ_lat, circ_lon, lat_camera, lon_camera, Form1.Globals.gps_radius);
                    gimplepitch = -GPS.RadiansToDegrees(Math.Atan(helix_altitude / distance));
                    gimblemode  = 2;
                }
                else
                {
                    heading = GPS.Mod_Angle(angle + 90);
                    if (radioCCW.Checked)
                    {
                        heading = GPS.Mod_Angle(heading + 180.0);
                    }
                }

                _wp.Add_Waypoint_List(new_list, circ_lat, circ_lon, helix_altitude, heading, curvesize, rotdir, gimblemode, gimplepitch, actions);

                angle = angle + angle_increment;
                count++;
            } while (count <= helix_num_points);

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, lat_home, lon_home, helix_start_alt, 0.0, curvesize, rotdir, gimblemode, gimplepitch, no_actions);
            }

            // Save Path

            if (_new_path & _first_pass)
            {
                string path_name = txtAddHelixPathName.Text;
                if (path_name == "")
                {
                    path_name = "Untitled - Helical";
                }
                _path.Add_Path(_wpg, _gmap, path_name, "Helical", new_list);
                _path       = _wpg.PathAt(_wpg.PathCount() - 1);
                _first_pass = false;
            }
            else
            {
                _path.waypoints = new_list;
                //_gmap.Delete_gMapPath(_path);
                //_gmap.Add_gMapPath(_path, false);
            }

            /*
             * if (_current_path_index != -1)
             * {
             *  _wpg.DeletePath(_wpg.PathAt(_current_path_index));
             * }
             * string path_name = txtAddHelixPathName.Text;
             * if (path_name == "") path_name = "Untitled - Helical";
             * _path.Add_Path(_wpg, _gmap, path_name, "Helical", new_list);
             * int index = _wpg.PathCount() - 1;
             * _current_path_index = index;
             * Models.Path path = _wpg.PathAt(index);
             * _path = path;
             * string exist_type = path.type;
             * bool exist_select = path.selected;
             * bool exist_visible = path.visible;
             * if (exist_type == "Helical")
             * {
             *  _wpg.ChangePathWP(index, new_list);
             *  string pathname = path.name;
             *  int id = path.id;
             *  string type = path.type;
             *  _gmap.Delete_gMapPath(path);
             *  Models.Path newpath = new Models.Path();
             *  newpath.name = pathname;
             *  newpath.id = id;
             *  newpath.type = type;
             *  newpath.selected = exist_select;
             *  newpath.visible = exist_visible;
             *  newpath.waypoints = new_list;
             *  _gmap.Add_gMapPath(path, false);
             * }
             */

            _gmap.ReDrawgMap();
            //_wpg.ChangePathWP(index, new_list);
            //cmbCircReuse.ResetText();
        }
        private void BuildPolyGridPath()
        {
            if (!_build)
            {
                return;
            }

            // Polygon Search Grid
            //

            double altitude = Convert.ToDouble(txtGridAlt.Text);

            _camera_width  = (2 * (Math.Tan(GPS.DegreesToRadians(_cam_ang_hor / 2)) * altitude));
            _camera_height = (2 * (Math.Tan(GPS.DegreesToRadians(_cam_ang_ver / 2)) * altitude));

            // Get Poly Name and points
            //
            Double[,] point_arr        = new double[1000, 3];
            WPG_Vector[,] poly_vectors = new WPG_Vector[1000, 2];
            WPG_Vector[,] path_vectors = new WPG_Vector[1000, 2];
            //int poly_index = cmbShape.SelectedIndex;
            if (_poly_index == -1)
            {
                MessageBox.Show("Error : Select a Polygon");
                return;
            }
            LinkedList <PolyPoint> points = new LinkedList <PolyPoint>();
            PolyPoint point = new PolyPoint();

            Models.Shape poly = _wpg.ShapeWithId(_poly_intid);
            rtbPoly.Clear();
            rtbPoly.AppendText("Polygon : " + poly.name + "\n");

            points = poly.points;
            int    poly_count = points.Count();
            double lat_max    = -1000.0;
            double lon_max    = -1000.0;
            double lat_min    = 1000.0;
            double lon_min    = 1000.0;

            //LinkedList<WayPoints> new_list = new LinkedList<WayPoints>();
            int[,] no_actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };
            int[,] pict_actions = new int[, ] {
                { 0, 2000 }, { 1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };


            for (int i = 0; i < poly_count; i++)
            {
                point = points.ElementAt(i);
                double lat = point.lat;
                double lon = point.lon;
                double alt = point.alt;
                //Add_Waypoint_List(new_list, lat, lon, alt, 0, 0, 0, 0, 0, no_actions);
                point_arr[i, 0] = lat;
                point_arr[i, 1] = lon;
                point_arr[i, 2] = alt;
                if (lat > lat_max)
                {
                    lat_max = lat;
                }
                if (lat < lat_min)
                {
                    lat_min = lat;
                }
                if (lon > lon_max)
                {
                    lon_max = lon;
                }
                if (lon < lon_min)
                {
                    lon_min = lon;
                }
            }
            double delta_lat = lat_max - lat_min;
            double delta_lon = lon_max - lon_min;
            // Generate Polygon Vectors
            int vector_count = 0;

            for (int i = 0; i < poly_count - 1; i++)
            {
                poly_vectors[vector_count, 0].lat = point_arr[i, 0];
                poly_vectors[vector_count, 0].lon = point_arr[i, 1];
                poly_vectors[vector_count, 1].lat = point_arr[i + 1, 0];
                poly_vectors[vector_count, 1].lon = point_arr[i + 1, 1];
                vector_count++;
            }

            rtbPoly.AppendText("Poly Vector Count : " + Convert.ToString(vector_count) + "\n");

            // Generate Potential Paths

            LinkedList <WayPoints> new_list = new LinkedList <WayPoints>();

            int path_count = 0;

            double head       = Convert.ToDouble(txtHeading.Text);
            double bearing    = head - 90.0;
            double path_delta = _camera_width * ((100 - _over_wid) / 100);
            double dist       = GPS.GPS_Distance(lat_min, lon_min, lat_max, lon_max, Form1.Globals.gps_radius);
            int    num_path   = Convert.ToInt16(dist / path_delta);
            double lat_base   = lat_min;
            double lon_base   = lon_min;

            if (head >= -90 & head < 90)
            {
                lat_base = lat_min;
                lon_base = lon_max;
            }

            // Generate First Potential Path
            double new_lat1 = GPS.GPS_Lat_BearDist(lat_base, lon_base, bearing, 1000, Form1.Globals.gps_radius);
            double new_lon1 = GPS.GPS_Lon_BearDist(lat_base, lon_base, new_lat1, bearing, 1000, Form1.Globals.gps_radius);
            double new_lat2 = GPS.GPS_Lat_BearDist(lat_base, lon_base, bearing, -1000, Form1.Globals.gps_radius);
            double new_lon2 = GPS.GPS_Lon_BearDist(lat_base, lon_base, new_lat2, bearing, -1000, Form1.Globals.gps_radius);
            double bear     = GPS.GPS_Bearing(new_lat1, new_lon1, new_lat2, new_lon2);

            // Generate Path Waypoints to show paths
            //_wp.Add_Waypoint_List(new_list, new_lat1, new_lon1, 30, 0, 0, 0, 0, 0, no_actions);
            //_wp.Add_Waypoint_List(new_list, new_lat2, new_lon2, 30, 0, 0, 0, 0, 0, no_actions);

            path_vectors[path_count, 0].lat     = new_lat1;
            path_vectors[path_count, 0].lon     = new_lon1;
            path_vectors[path_count, 0].bearing = bearing;
            path_vectors[path_count, 1].lat     = new_lat2;
            path_vectors[path_count, 1].lon     = new_lon2;
            path_vectors[path_count, 1].bearing = bearing;
            path_count++;
            // Generate remaining Paths
            double new_lat_base, new_lon_base;

            for (int i = 0; i < num_path; i++)
            {
                new_lat_base = GPS.GPS_Lat_BearDist(lat_base, lon_base, 0 + bearing, path_delta * i, Form1.Globals.gps_radius);
                new_lon_base = GPS.GPS_Lon_BearDist(lat_base, lon_base, new_lat_base, 0 + bearing, path_delta * i, Form1.Globals.gps_radius);

                new_lat1 = GPS.GPS_Lat_BearDist(new_lat_base, new_lon_base, bearing + 90, 1000, Form1.Globals.gps_radius);
                new_lon1 = GPS.GPS_Lon_BearDist(new_lat_base, new_lon_base, new_lat1, bearing + 90, 1000, Form1.Globals.gps_radius);
                new_lat2 = GPS.GPS_Lat_BearDist(new_lat_base, new_lon_base, bearing + 270, 1000, Form1.Globals.gps_radius);
                new_lon2 = GPS.GPS_Lon_BearDist(new_lat_base, new_lon_base, new_lat2, bearing + 270, 1000, Form1.Globals.gps_radius);
                bear     = GPS.GPS_Bearing(new_lat1, new_lon1, new_lat2, new_lon2);

                //_wp.Add_Waypoint_List(new_list, new_lat1, new_lon1, 30, 0, 0, 0, 0, 0, no_actions);
                //_wp.Add_Waypoint_List(new_list, new_lat2, new_lon2, 30, 0, 0, 0, 0, 0, no_actions);

                path_vectors[path_count, 0].lat     = new_lat1;
                path_vectors[path_count, 0].lon     = new_lon1;
                path_vectors[path_count, 0].bearing = bearing;
                path_vectors[path_count, 1].lat     = new_lat2;
                path_vectors[path_count, 1].lon     = new_lon2;
                path_vectors[path_count, 1].bearing = bearing;
                path_count++;
            }

            // Check for intersections of potential paths with polygon borders

            Form1.WPG_PointVector pvec1 = new Form1.WPG_PointVector();
            Form1.WPG_PointVector pvec2 = new Form1.WPG_PointVector();
            bool lines_intersect;
            bool segments_intersect;

            PointF closep1;
            PointF closep2;
            bool   firstpnt   = true;
            bool   firstleg   = true;
            double heading    = 0.0;
            double lat1       = 0.0;
            double lon1       = 0.0;
            int    gimblemode = 0;
            double curvesize  = 0;
            double rotdir     = 0;
            bool   startend   = chkRectHome.Checked;
            bool   video      = radioVideo.Checked;
            int    leg_count  = 0;

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, _lat, _lon, altitude, 0.0, curvesize, rotdir, gimblemode, 0.0, no_actions);
            }

            for (int i = 1; i < path_count; i++)
            {
                pvec1.pnt1.lat = path_vectors[i, 0].lat;
                pvec1.pnt1.lon = path_vectors[i, 0].lon;
                pvec1.pnt2.lat = path_vectors[i, 1].lat;
                pvec1.pnt2.lon = path_vectors[i, 1].lon;

                for (int j = 0; j < vector_count; j++)
                {
                    PointF intersection;
                    pvec2.pnt1.lat = poly_vectors[j, 0].lat;
                    pvec2.pnt1.lon = poly_vectors[j, 0].lon;
                    pvec2.pnt2.lat = poly_vectors[j, 1].lat;
                    pvec2.pnt2.lon = poly_vectors[j, 1].lon;

                    GPS.GPS_Intersection(pvec1, pvec2, out lines_intersect, out segments_intersect, out intersection, out closep1, out closep2);
                    if (lines_intersect & segments_intersect)
                    {
                        if (firstpnt)
                        {
                            lat1     = Convert.ToDouble(intersection.X);
                            lon1     = Convert.ToDouble(intersection.Y);
                            firstpnt = false;
                        }
                        else
                        {
                            double lat = Convert.ToDouble(intersection.X);
                            double lon = Convert.ToDouble(intersection.Y);
                            if (firstleg)
                            {
                                heading  = GPS.GPS_Bearing(lat1, lon1, lat, lon);
                                firstleg = false;
                            }
                            _wp.Add_Leg_List(new_list, lat1, lon1, lat, lon, altitude, heading, 0, 0, 0, 0, pict_actions, video, _camera_height, _over_hgt);
                            leg_count++;
                            firstpnt = true;
                        }
                    }
                }
            }

            if (startend)
            {
                _wp.Add_Waypoint_List(new_list, _lat, _lon, altitude, 0.0, curvesize, rotdir, gimblemode, 0.0, no_actions);
            }

            rtbPoly.AppendText("Leg Count :" + Convert.ToString(leg_count) + "\n");
            int wp_count = new_list.Count;

            rtbPoly.AppendText("Waypoint Count :" + Convert.ToString(wp_count) + "\n");

            // Save Path

            // Save Path

            if (_new_path & _first_pass)
            {
                String path_name = txtPolyPathName.Text;
                if (path_name == "")
                {
                    path_name = "Untitled - Polygon";
                }
                _path.Add_Path(_wpg, _gmap, path_name, "Polygon", new_list);
                _path          = _wpg.PathAt(_wpg.PathCount() - 1);
                _current_intid = _path.internal_id;
                _first_pass    = false;
            }
            else
            {
                _wpg.ChangePathWPIntId(_current_intid, new_list);
                //Models.Path path = _wpg.PathAt(_current_path_index);
                //_gmap.Delete_gMapPath(path);
                //_gmap.Add_gMapPath(path, false);
            }
            _poly.visible = true;
            _gmap.ReDrawgMap();
        }