public void Add_Waypoint_List(LinkedList <WayPoints> list, double lat, double lon, double alt, double heading, double curvesize, double rotdir, int gimblemode, double gimblepitch, int[,] actions)
{
WayPoints waypoint = new WayPoints();
waypoint.lat = lat;
waypoint.lon = lon;
waypoint.alt = alt;
waypoint.head = heading;
waypoint.curvesize = curvesize;
waypoint.rotationdir = rotdir;
waypoint.gimblemode = gimblemode;
waypoint.gimblepitch = gimblepitch;
waypoint.actions = actions;
list.AddLast(waypoint);
}
public void Insert_Waypoint_List(Waypoint_Path_Gen wpg, double lat, double lon, bool before)
{
// Find Selected WP
for (int path_id = 0; path_id < wpg.PathCount(); path_id++)
{
Path path = wpg.PathAt(path_id);
LinkedList <WayPoints> wp_list = path.waypoints;
LinkedListNode <WayPoints> node = wp_list.First;
LinkedListNode <WayPoints> next_node;
while (node != null)
{
next_node = node.Next;
if (node.Value.selected)
{
WayPoints wp_new = new WayPoints();
wp_new.lat = lat;
wp_new.lon = lon;
wp_new.alt = node.Value.alt;
wp_new.head = node.Value.head;
wp_new.curvesize = node.Value.curvesize;
wp_new.rotationdir = node.Value.rotationdir;
wp_new.gimblemode = node.Value.gimblemode;
wp_new.gimblepitch = node.Value.gimblepitch;
wp_new.actions = node.Value.actions;
wp_new.selected = true;
wp_new.visible = true;
wp_new.path_int_id = path.internal_id;
if (before)
{
wp_list.AddBefore(node, wp_new);
}
else
{
wp_list.AddAfter(node, wp_new);
}
node.Value.selected = false;
return;
}
node = next_node;
}
}
}
public void ReversePathWP(int i)
{
// Build new wp list
Models.Path path = path_list.ElementAt(i);
LinkedList <WayPoints> wplist = path.waypoints;
LinkedList <WayPoints> wpnew = new LinkedList <WayPoints>();
int count = wplist.Count;
int index = count - 1;
while (index >= 0)
{
WayPoints wp = wplist.ElementAt(index);
WayPoints wp_new = new WayPoints();
wp_new = wp;
wpnew.AddLast(wp_new);
index--;
}
// Build New Path Element
Models.Path newpath = new Models.Path();
newpath.name = path.name;
newpath.type = path.type;
newpath.selected = path.selected;
newpath.visible = path.visible;
newpath.waypoints = wpnew;
// Now insert new before the old and delete the old
LinkedListNode <Models.Path> node = path_list.First;
LinkedListNode <Models.Path> next_node;
count = 0;
while (node != null)
{
next_node = node.Next;
if (count == i)
{
path_list.AddBefore(node, newpath);
next_node = node.Next;
path_list.Remove(path);
}
count++;
node = next_node;
}
}
public void Add_Leg_List(LinkedList <WayPoints> list, double lat1, double lon1, double lat2, double lon2,
double alt, double heading, double curvesize, double rotdir, int gimblemode, double gimblepitch, int[,] actions,
bool video, double camera_increment, double overlap)
{
// See if we have to reverse the points
WayPoints wp = new WayPoints();
int list_count = list.Count;
if (list_count > 0)
{
double last_lat = list.Last.Value.lat;
double last_lon = list.Last.Value.lon;
double dist1 = GPS.GPS_Distance(last_lat, last_lon, lat1, lon1, Form1.Globals.gps_radius);
double dist2 = GPS.GPS_Distance(last_lat, last_lon, lat2, lon2, Form1.Globals.gps_radius);
if (dist2 < dist1)
{
double temp = lat1;
lat1 = lat2;
lat2 = temp;
temp = lon1;
lon1 = lon2;
lon2 = temp;
}
}
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 }
};
if (video)
{
// If in Video mode only output start and end points
wp.Add_Waypoint_List(list, lat1, lon1, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
wp.Add_Waypoint_List(list, lat2, lon2, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
}
else
{
// Photo Mode - Break into seperate segments
// Output first WP
wp.Add_Waypoint_List(list, lat1, lon1, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
camera_increment = camera_increment * (1 - overlap / 100.0);
double distance = GPS.GPS_Distance(lat1, lon1, lat2, lon2, Form1.Globals.gps_radius);
double bearing = GPS.GPS_Bearing(lat1, lon1, lat2, lon2);
double new_lat, new_lon, last_lat, last_lon;
int num_wp = (int)(distance / camera_increment);
if (num_wp != 0)
{
if (num_wp == 1)
{
new_lat = GPS.GPS_Lat_BearDist(lat1, lon1, bearing, distance / 2, Form1.Globals.gps_radius);
new_lon = GPS.GPS_Lon_BearDist(lat1, lon1, new_lat, bearing, distance / 2, Form1.Globals.gps_radius);
wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
}
else
{
double width = (num_wp - 1) * camera_increment;
double dist_diff = distance - width;
new_lat = GPS.GPS_Lat_BearDist(lat1, lon1, bearing, dist_diff / 2, Form1.Globals.gps_radius);
new_lon = GPS.GPS_Lon_BearDist(lat1, lon1, new_lat, bearing, dist_diff / 2, Form1.Globals.gps_radius);
wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
last_lat = new_lat;
last_lon = new_lon;
for (int i = 0; i < num_wp - 1; i++)
{
new_lat = GPS.GPS_Lat_BearDist(last_lat, last_lon, bearing, camera_increment, Form1.Globals.gps_radius);
new_lon = GPS.GPS_Lon_BearDist(last_lat, last_lon, new_lat, bearing, camera_increment, Form1.Globals.gps_radius);
wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
last_lat = new_lat;
last_lon = new_lon;
}
}
}
// Output last WP
wp.Add_Waypoint_List(list, lat2, lon2, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
}
}
public void ReadXml_Path(string file_name)
{
XmlDocument xmlDoc = new XmlDocument();
xmlDoc.Load(file_name);
string dialog_text = "XMLReadPath\n\n";
// Get Paths
XmlNodeList pathlist = xmlDoc.DocumentElement.SelectNodes("/WPG/Path");
int path_count = pathlist.Count;
dialog_text = dialog_text + "Path Count : " + path_count + "\n";
ClearPath();
foreach (XmlNode path_node in pathlist)
{
Models.Path path = new Models.Path();
path.name = path_node.SelectSingleNode("Name").InnerText;
path.type = path_node.SelectSingleNode("Type").InnerText;
path.internal_id = Convert.ToInt16(path_node.SelectSingleNode("InternalId").InnerText);
if (path.type == "Circular")
{
CircularGUI gui = new CircularGUI();
XmlNodeList GUI_node = path_node.SelectNodes("./GUI");
foreach (XmlNode node in GUI_node)
{
gui.CW = Convert.ToBoolean(node.SelectSingleNode("CW").InnerText);
gui.name = node.SelectSingleNode("Name").InnerText;
gui.lat = Convert.ToDouble(node.SelectSingleNode("Lat").InnerText);
gui.lon = Convert.ToDouble(node.SelectSingleNode("Lon").InnerText);
gui.altitude = Convert.ToDouble(node.SelectSingleNode("Altitude").InnerText);
gui.radius = Convert.ToDouble(node.SelectSingleNode("Radius").InnerText);
gui.numpoints = Convert.ToInt16(node.SelectSingleNode("NumPoints").InnerText);
gui.fullcirc = Convert.ToBoolean(node.SelectSingleNode("FullCircle").InnerText);
gui.start_angle = Convert.ToDouble(node.SelectSingleNode("StartAngle").InnerText);
gui.circ_span = Convert.ToDouble(node.SelectSingleNode("CircleSpan").InnerText);
gui.startend = Convert.ToBoolean(node.SelectSingleNode("StartEnd").InnerText);
gui.poimode = Convert.ToBoolean(node.SelectSingleNode("POIMode").InnerText);
gui.poiname = node.SelectSingleNode("POIName").InnerText;
}
path.circgui = gui;
}
if (path.type == "Helical")
{
HelicalGUI gui = new HelicalGUI();
XmlNodeList GUI_node = path_node.SelectNodes("./GUI");
foreach (XmlNode node in GUI_node)
{
gui.CW = Convert.ToBoolean(node.SelectSingleNode("CW").InnerText);
gui.name = node.SelectSingleNode("Name").InnerText;
gui.lat = Convert.ToDouble(node.SelectSingleNode("Lat").InnerText);
gui.lon = Convert.ToDouble(node.SelectSingleNode("Lon").InnerText);
gui.start_alt = Convert.ToDouble(node.SelectSingleNode("StartAltitude").InnerText);
gui.end_alt = Convert.ToDouble(node.SelectSingleNode("EndAltitude").InnerText);
gui.start_rad = Convert.ToDouble(node.SelectSingleNode("StartRadius").InnerText);
gui.end_rad = Convert.ToDouble(node.SelectSingleNode("EndRadius").InnerText);
gui.start_angle = Convert.ToDouble(node.SelectSingleNode("StartAngle").InnerText);
gui.helix_span = Convert.ToDouble(node.SelectSingleNode("HelixSpan").InnerText);
gui.num_points = Convert.ToInt16(node.SelectSingleNode("NumPoints").InnerText);
gui.startend = Convert.ToBoolean(node.SelectSingleNode("StartEnd").InnerText);
gui.poimode = Convert.ToBoolean(node.SelectSingleNode("POIMode").InnerText);
gui.poiname = node.SelectSingleNode("POIName").InnerText;
}
path.helixgui = gui;
}
if (path.type == "Rectangular")
{
RectanglarGUI gui = new RectanglarGUI();
XmlNodeList GUI_node = path_node.SelectNodes("./GUI");
foreach (XmlNode node in GUI_node)
{
gui.name = node.SelectSingleNode("Name").InnerText;
gui.video = Convert.ToBoolean(node.SelectSingleNode("Video").InnerText);
gui.startend = Convert.ToBoolean(node.SelectSingleNode("StartEnd").InnerText);
gui.altitude = Convert.ToDouble(node.SelectSingleNode("Altitude").InnerText);
gui.heading = Convert.ToDouble(node.SelectSingleNode("Heading").InnerText);
gui.length = Convert.ToDouble(node.SelectSingleNode("Length").InnerText);
gui.width = Convert.ToDouble(node.SelectSingleNode("Width").InnerText);
gui.single = Convert.ToBoolean(node.SelectSingleNode("SinglePath").InnerText);
gui.poimode = Convert.ToBoolean(node.SelectSingleNode("POIMode").InnerText);
gui.poiname = node.SelectSingleNode("POIName").InnerText;
}
path.rectanglegui = gui;
}
if (path.type == "Polygon")
{
PolygonGridGUI gui = new PolygonGridGUI();
XmlNodeList GUI_node = path_node.SelectNodes("./GUI");
foreach (XmlNode node in GUI_node)
{
gui.name = node.SelectSingleNode("Name").InnerText;
gui.video = Convert.ToBoolean(node.SelectSingleNode("Video").InnerText);
gui.startend = Convert.ToBoolean(node.SelectSingleNode("StartEnd").InnerText);
gui.altitude = Convert.ToDouble(node.SelectSingleNode("Altitude").InnerText);
gui.heading = Convert.ToDouble(node.SelectSingleNode("Heading").InnerText);
gui.polyname = node.SelectSingleNode("PolyName").InnerText;
gui.poly_internal_id = Convert.ToInt16(node.SelectSingleNode("Poly_IntID").InnerText);
}
path.polygridgui = gui;
}
path.selected = false;
path.visible = false;
LinkedList <WayPoints> way_list = new LinkedList <WayPoints>();
XmlNodeList way_nodes = path_node.SelectNodes("./Waypoint");
int way_count = way_nodes.Count;
dialog_text = dialog_text + "Path : " + path.name + "Type : " + path.type + "\n";
dialog_text = dialog_text + "Waypoint Count : " + Convert.ToString(way_count) + "\n";
//MessageBox.Show(dialog_text, "xxx");
way_count = 0;
foreach (XmlNode wp_node in way_nodes)
{
WayPoints waypoint = new WayPoints();
waypoint.lat = Convert.ToDouble(wp_node.SelectSingleNode("Lat").InnerText);
waypoint.lon = Convert.ToDouble(wp_node.SelectSingleNode("Lon").InnerText);
waypoint.alt = Convert.ToDouble(wp_node.SelectSingleNode("Alt").InnerText);
waypoint.head = Convert.ToDouble(wp_node.SelectSingleNode("Heading").InnerText);
waypoint.curvesize = Convert.ToDouble(wp_node.SelectSingleNode("CurveSize").InnerText);
waypoint.rotationdir = Convert.ToDouble(wp_node.SelectSingleNode("RotationDir").InnerText);
waypoint.gimblemode = Convert.ToInt16(wp_node.SelectSingleNode("GimbleMode").InnerText);
waypoint.gimblepitch = Convert.ToDouble(wp_node.SelectSingleNode("GimblePitch").InnerText);
XmlNodeList action_nodes = wp_node.SelectNodes("./Action");
int action_count = action_nodes.Count;
dialog_text = dialog_text + "Waypoint : " + Convert.ToString(way_count) + "\n";
dialog_text = dialog_text + "Action Count : " + Convert.ToString(action_count) + "\n";
int[,] actions_arr = new int[100, 2];
int i = 0;
int act_type, act_param;
foreach (XmlNode act_node in action_nodes)
{
act_type = Convert.ToInt16(act_node.SelectSingleNode("Type").InnerText);
act_param = Convert.ToInt16(act_node.SelectSingleNode("Param").InnerText);
dialog_text = dialog_text + "Action Count : " + Convert.ToString(i);
dialog_text = dialog_text + ", Action Type : " + Convert.ToString(act_type);
dialog_text = dialog_text + ", Action Param : " + Convert.ToString(act_param) + "\n";
actions_arr[i, 0] = act_type;
actions_arr[i, 1] = act_param;
i++;
}
waypoint.actions = actions_arr;
way_list.AddLast(waypoint);
way_count++;
}
//MessageBox.Show(dialog_text, "xxx");
path.waypoints = way_list;
AddPathId(path.internal_id, path);
path_count++;
}
}
public void SplitPath(bool before)
{
Path path;
Path path1 = new Path();
Path path2 = new Path();
string path_name;
int selected_wp_index = -1;
int selected_path_index = -1;
Path selected_path;
LinkedList <WayPoints> wp_list;
// Get Selected Waypoint
for (int i = 0; i < PathCount(); i++)
{
path = PathAt(i);
wp_list = path.waypoints;
for (int j = 0; j < wp_list.Count; j++)
{
if (wp_list.ElementAt(j).selected)
{
selected_wp_index = j;
break;
}
}
if (selected_wp_index != -1)
{
selected_path_index = i;
selected_path = PathAt(i);
break;
}
}
if (selected_path_index == -1 | selected_wp_index == -1)
{
return;
}
// Create two new paths
path = PathAt(selected_path_index);
path_name = path.name;
path1.name = path_name + "-1";
path2.name = path_name + "-2";
path1.type = path.type + " - Split";
path2.type = path.type + " - Split";
path1.selected = false;
path2.selected = false;
path1.visible = true;
path2.visible = true;
// Create two Waypoint Lists
wp_list = path.waypoints;
LinkedList <WayPoints> wp_list1 = new LinkedList <WayPoints>();
LinkedList <WayPoints> wp_list2 = new LinkedList <WayPoints>();
int wp_count = wp_list.Count;
if (wp_count <= 1)
{
return;
}
// Split at first Waypoint
if (selected_wp_index == 0)
{
WayPoints wpnew = new WayPoints();
wpnew = wp_list.ElementAt(0);
wp_list1.AddLast(wpnew);
for (int i = 1; i < wp_count - 1; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list2.AddLast(wpnew);
}
}
// Split at last wp
if (selected_wp_index == wp_count - 1)
{
WayPoints wpnew;
for (int i = 0; i < wp_count - 1; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list1.AddLast(wpnew);
}
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(wp_count - 1);
wp_list2.AddLast(wpnew);
}
// Split at Middle
if (selected_wp_index > 0 & selected_wp_index < wp_count - 1)
{
WayPoints wpnew;
if (before)
{
for (int i = 0; i < selected_wp_index; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list1.AddLast(wpnew);
}
for (int i = selected_wp_index; i < wp_count; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list2.AddLast(wpnew);
}
}
else
{
for (int i = 0; i <= selected_wp_index; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list1.AddLast(wpnew);
}
for (int i = selected_wp_index + 1; i < wp_count; i++)
{
wpnew = new WayPoints();
wpnew = wp_list.ElementAt(i);
wp_list2.AddLast(wpnew);
}
}
}
// Add Waypoints to paths
path1.waypoints = wp_list1;
path2.waypoints = wp_list2;
// Delete original Path
DeletePath(path);
// Add two new paths
AddPath(path1);
AddPath(path2);
}
