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