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 BuildKMLPath()
{
// Create Path
if (_current_path_index != -1)
{
_wpg.DeletePath(_wpg.PathAt(_current_path_index));
}
string path_name = txtKMLName.Text;
if (path_name == "")
{
path_name = _kml_filename;
}
LinkedList <WayPoints> wp_list = new LinkedList <WayPoints>();
double lat, lon, alt;
int gimblemode = 0;
double gimblepitch = 0;
double curvesize = 0;
double rotdir = 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 }
};
LinkedList <WayPoints> new_list = new LinkedList <WayPoints>();
/* Generate Waypoints for each coordinate */
for (int i = 0; i < _kml_point_count; i++)
{
lat = _kml_points[i, 0];
lon = _kml_points[i, 1];
alt = _kml_points[i, 2];
double heading = 0.0;
double poi_lat = 0;
double poi_lon = 0;
/* Calculate Heading for each Waypoint */
if (chKLMPOI.Checked)
{
int poi_index = cmbKLMPOI.SelectedIndex;
if (poi_index == -1)
{
poi_lat = _lat;
poi_lon = _lon;
}
else
{
POIPoints tmp_point = _wpg.POIPointAt(poi_index);
poi_lat = tmp_point.lat;
poi_lon = tmp_point.lon;
}
heading = GPS.GPS_Bearing(lat, lon, poi_lat, poi_lon);
gimblemode = 2;
double distance = GPS.GPS_Distance(lat, lon, poi_lat, poi_lon, Form1.Globals.gps_radius);
gimblepitch = -GPS.RadiansToDegrees(Math.Atan(alt / distance));
gimblemode = 2;
}
else
{
if (i != _kml_point_count - 1)
{
double lat_next = _kml_points[i + 1, 0];
double lon_next = _kml_points[i + 1, 1];
heading = GPS.GPS_Bearing(lat, lon, lat_next, lon_next);
}
}
_wp.Add_Waypoint_List(new_list, lat, lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
//dgvWaypoints.Rows.Add(Globals.waypoint_count + i+1, Convert.ToString(lat), Convert.ToString(lon), Convert.ToString(alt));
}
_path.Add_Path(_wpg, _gmap, path_name, "KML", new_list);
int index = _wpg.PathCount() - 1;
_current_path_index = index;
Models.Path path = _wpg.PathAt(index);
string exist_type = path.type;
bool exist_select = path.selected;
bool exist_visible = path.visible;
if (exist_type == "KML")
{
_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();
GMAPTree.Update_GMapTree(_wpg, _treeview);
}
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();
}