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