diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 5a83394880..8b168a818e 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -119,7 +119,8 @@ approaching_time CDATA #IMPLIED gaz CDATA #IMPLIED> +wp CDATA #REQUIRED +radius CDATA #IMPLIED> + let x1 = p1.G2D.x2D and y1 = p1.G2D.y2D + and x2 = p2.G2D.x2D and y2 = p2.G2D.y2D in + s#set [`POINTS [|x1;y1;x2;y2|]] + + let arc_from_points = fun (geomap:MapCanvas.widget) c l f s -> + let cl = G2D.vect_make c l + and cf = G2D.vect_make c f in + let pol_cl = G2D.cart2polar cl in + let al = pol_cl.G2D.theta2D + and af = (G2D.cart2polar cf).G2D.theta2D in + let xc = c.G2D.x2D and yc = c.G2D.y2D in + let (a1, a2) = if s > 0. then (al, af) else (af, al) in + geomap#arc ~nb_points:10 ~fill_color:"blue" ~width:2 (xc,yc) pol_cl.G2D.r2D a1 a2 + + (** Update path after waypoint move *) + let update_path = fun (geomap:MapCanvas.widget) path waypoint -> + let n = waypoint#name in + let rec loop = function + [] -> failwith (sprintf "update_path: %s not found" n) + | [p] -> [p] + | ((wp, Some arc, Some seg, _f, radius) as pp)::wps -> + let new_wps = if wp#name = waypoint#name then wps else loop wps in + begin + match new_wps with + [] -> failwith "unreachable" + | (wp1, _arc1, _seg1, f1, _r1)::new_wps' -> (* Previous *) + let wp1_2D = geomap#pt2D_of wp1#pos + and wp_2D = geomap#pt2D_of wp#pos in + match new_wps' with + [] -> (* wp is the second point: simple segment *) + set_segment seg wp1_2D wp_2D; + (wp, Some arc, Some seg, wp1_2D, radius)::new_wps + | _ -> (* At least 2 points before *) + let (c, f, s) = G2D.arc_segment f1 wp1_2D wp_2D radius in + set_segment seg f wp_2D; + arc#destroy (); + let new_arc = arc_from_points geomap c wp1_2D f s in + (wp, Some new_arc, Some seg, f, radius)::new_wps + end + | _ -> failwith "update_path" in + path := loop !path + + + let path_button = fun (geomap:MapCanvas.widget) (xw, yw) -> let geo = geomap#of_world (xw, yw) in let wp = create_wp geomap geo in - if !path = [] then + let cur_path = !path in + wp#connect (fun () -> update_path geomap cur_path wp); + if ! !path = [] then cur_f := {G2D.x2D=xw; G2D.y2D=yw}; - path := (wp, !cur_arc, !cur_seg, !cur_f) :: !path; + cur_path := (wp, !cur_arc, !cur_seg, !cur_f, !radius) :: ! cur_path; cur_arc := Some (GnoCanvas.line geomap#canvas#root); cur_seg := Some (geomap#segment ~fill_color:"blue" ~width:2 geo geo) let path_notify (geomap:MapCanvas.widget) (xw, yw) = - match !path with - [] -> false - | (wp1, _, _, ll) :: p -> - let (xl, yl) = geomap#world_of wp1#pos in + match ! !path with + [] -> false (** Empty path: nothing to do *) + | (wp1, _, _, ll,_) :: p -> (** Last is wp1 *) begin match !cur_seg with None -> failwith "path_notify" | Some segment -> + let l = geomap#pt2D_of wp1#pos + and cur_2D = {G2D.x2D = xw; y2D = yw } in match p with - [] -> - segment#set [`POINTS [|xl;yl;xw;yw|]] - | _::_ -> + [] -> (** Only 1 point in the current path:add a simple segment*) + set_segment segment l cur_2D; + | _::_ -> (** Already 2 points: add an arc and a segment *) match !cur_arc with None -> failwith "path_notify" | Some arc -> arc#destroy (); - let l = {G2D.x2D = xl; y2D = yl } - and p = {G2D.x2D = xw; y2D = yw } in - let (c, f, s) = G2D.arc_segment ll l p !radius in - let xf = f.G2D.x2D and yf = f.G2D.y2D in - segment#set [`POINTS [|xf;yf;xw;yw|]]; - let cl = G2D.vect_make c l - and cf = G2D.vect_make c f in - let al = (G2D.cart2polar cl).G2D.theta2D - and af = (G2D.cart2polar cf).G2D.theta2D in - let xc = c.G2D.x2D and yc = c.G2D.y2D in - let (a1, a2) = if s > 0. then (al, af) else (af, al) in - let arc = geomap#arc ~nb_points:10 ~fill_color:"blue" ~width:2 (xc,yc) !radius a1 a2 in + let (c, f, s) = G2D.arc_segment ll l cur_2D !radius in + set_segment segment f cur_2D; + let arc = arc_from_points geomap c l f s in cur_arc := Some arc; cur_f := f end; true + let path_close = fun () -> let destroy = fun ref -> @@ -681,11 +722,17 @@ module Edit = struct | Some s -> s#destroy (); ref := None in destroy cur_arc; destroy cur_seg; - path := [] + begin + match !current_fp with + None -> () + | Some (fp, _) -> + fp#insert_path (List.map (fun (wp,_,_,_,r) -> (wp, r)) (List.rev ! !path)); + end; + path := ref [] let path_change_radius = function - `UP -> radius := !radius *. 1.25 - | `DOWN -> radius := !radius /. 1.25 + `UP -> radius := !radius *. 1.1 + | `DOWN -> radius := !radius /. 1.1 | _ -> () end (** Edit module *) diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml index dc97c82249..8ca1fc534a 100644 --- a/sw/lib/ocaml/mapCanvas.ml +++ b/sw/lib/ocaml/mapCanvas.ml @@ -184,6 +184,10 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef ( end | None -> failwith "#world_of : no georef" + method pt2D_of = fun wgs84 -> + let (x, y) = self#world_of wgs84 in + {G2D.x2D = x; y2D = y} + method of_world = fun (wx, wy) -> match georef with Some georef -> begin diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml index 69cb382395..dfa5f082ec 100644 --- a/sw/lib/ocaml/mapFP.ml +++ b/sw/lib/ocaml/mapFP.ml @@ -47,6 +47,35 @@ let update_wp utm_ref wp = function with _ -> () +let iter_stages = fun f xml_tree -> + let xml_blocks = XmlEdit.child (XmlEdit.root xml_tree) "blocks" in + let rec loop = fun n -> + f n; + List.iter loop (XmlEdit.children n) in + loop xml_blocks + +let try_replace_attrib = fun node tag prev_v v -> + try + if XmlEdit.attrib node tag = prev_v then + XmlEdit.set_attrib node (tag, v) + with + Not_found -> () + +(** Update all the references to waypoint names (attribute "wp") *) +let update_wp_refs previous_name xml_tree = function + XmlEdit.Deleted -> () (** FIXME *) + | XmlEdit.New_child _ -> () + | XmlEdit.Modified attribs -> + try + let new_name = assoc_nocase "name" attribs in + let update = fun node -> + try_replace_attrib node "wp" !previous_name new_name; + try_replace_attrib node "from" !previous_name new_name in + iter_stages update xml_tree; + previous_name := new_name + with + Not_found -> () + let waypoints_node = fun xml_tree -> let xml_root = XmlEdit.root xml_tree in XmlEdit.child xml_root "waypoints" @@ -67,6 +96,7 @@ let new_wp = fun xml_tree waypoints utm_ref ?(alt = 0.) node -> let name = XmlEdit.attrib node "name" in let wp = MapWaypoints.waypoint waypoints ~name ~alt wgs84 in XmlEdit.connect node (update_wp utm_ref wp); + XmlEdit.connect node (update_wp_refs (ref name) xml_tree); wp#connect (fun () -> update_xml xml_tree utm_ref wp); wp @@ -74,6 +104,10 @@ let gensym = let x = ref 0 in fun p -> incr x; Printf.sprintf "%s%d" p !x +let rec new_gensym = fun p l -> + let s = gensym p in + if List.mem s l then new_gensym p l else s + class flight_plan = fun geomap color fp_dtd xml -> (** Xml Editor *) @@ -123,12 +157,27 @@ class flight_plan = fun geomap color fp_dtd xml -> method waypoints = XmlEdit.children (waypoints_node xml_tree_view) method xml = XmlEdit.xml_of_view xml_tree_view method add_waypoint (geo:geographic) = - let name = gensym "wp" in + let wpt_names = List.map (fun n -> XmlEdit.attrib n "name") (XmlEdit.children xml_wpts) in + let name = new_gensym "wp" wpt_names in let utm = utm_of WGS84 geo in let (dx, dy) = utm_sub utm utm0 in let node = XmlEdit.add_child xml_wpts "waypoint" ["x",sof dx;"y",sof dy;"name",name] in create_wp node + method insert_path = fun path -> + let xml_block = + try XmlEdit.parent (XmlEdit.selection xml_tree_view) "block" with + _ -> + let xml_blocks = XmlEdit.child xml_root "blocks" in + XmlEdit.child xml_blocks "block" in + let path_node = XmlEdit.add_child xml_block "path" ["radius", "42."] in + List.iter + (fun ((wp:MapWaypoints.waypoint), r) -> + let n = XmlEdit.add_child path_node "path_point" ["wp", wp#name; "radius", sof r] in + () + ) + path + initializer ( (** Create a graphic waypoint when it is created from the xml editor *) XmlEdit.connect xml_wpts (function XmlEdit.New_child node -> ignore (create_wp node) | _ -> ()) diff --git a/sw/lib/ocaml/mapFP.mli b/sw/lib/ocaml/mapFP.mli index de99063ca1..db21403998 100644 --- a/sw/lib/ocaml/mapFP.mli +++ b/sw/lib/ocaml/mapFP.mli @@ -41,4 +41,5 @@ class flight_plan : method window : GWindow.window method waypoints : XmlEdit.node list method xml : Xml.xml + method insert_path : (MapWaypoints.waypoint * float) list -> unit end diff --git a/sw/lib/ocaml/xmlEdit.ml b/sw/lib/ocaml/xmlEdit.ml index 181390a8df..d487d565a0 100644 --- a/sw/lib/ocaml/xmlEdit.ml +++ b/sw/lib/ocaml/xmlEdit.ml @@ -35,8 +35,9 @@ external gtk_tree_view_get_drag_dest_row : 'a Gtk.obj -> Gtk.tree_path * gtkTree open Printf type tag = string -type attributes = (string * string) list -type t = GTree.tree_store +type attribute = string * string +type attributes = attribute list +type t = GTree.tree_store * GTree.view type node = GTree.tree_store * Gtk.tree_path let cols = new GTree.column_list @@ -190,6 +191,12 @@ let attr_submenu = fun menuitem tag dtd connect -> submenu menuitem (allowed_attributes tag dtd) connect +let selection = fun (tree_store, tree_view) -> + match tree_view#selection#get_selected_rows with + path::_ -> + tree_store, path + | _ -> raise Not_found + let attribs_menu_popup = fun dtd (tree_view:GTree.view) (model:GTree.tree_store) (attrib_row:Gtk.tree_iter) -> let menu = GMenu.menu () in let menuitem = GMenu.menu_item ~label:"Delete" ~packing:menu#append () in @@ -243,7 +250,7 @@ let add_context_menu = fun model view ?noselection_menu menu -> else false) -let root = fun (model:t) -> +let root = fun ((model:GTree.tree_store), _) -> match model#get_iter_first with None -> invalid_arg "XmlEdit.root" | Some i -> (model, model#get_path i) @@ -257,6 +264,17 @@ let set_attribs = fun ((model, path):node) attribs -> let row = model#get_iter path in model#set ~row ~column:attributes attribs +let rec replace_assoc a v = function + [] -> [(a, v)] + | (a', v')::l -> + if a = String.uppercase a' + then (a, v)::l + else (a', v')::replace_assoc a v l + +let set_attrib = fun node (a, v) -> + let atbs = attribs node in + set_attribs node (replace_assoc (String.uppercase a) v atbs) + let attrib = fun node at -> let at = String.uppercase at in let ats = attribs node in @@ -304,6 +322,17 @@ let child = fun ((model, path):node) (t:string) -> loop () else failwith (sprintf "XmlEdit.child: %s" t) + +let rec parent = fun ((model, path):node) (t:string) -> + let row = model#get_iter path in + let tag = model#get ~row ~column:tag_col in + if tag = t then + (model, path) + else + match model#iter_parent row with + None -> failwith (sprintf "XmlEdit.parent: %s" t) + | Some p -> + parent (model, model#get_path p) t let delete = fun (model, path) -> @@ -314,13 +343,13 @@ let delete = fun (model, path) -> let add_child = fun ((model, path):node) tag attribs -> let parent = model#get_iter path in let row = model#append ~parent () in - model#set ~row ~column:tag_col tag; - model#set ~row ~column:attributes attribs; + set_xml model row (Xml.Element (tag, attribs, [])); model, model#get_path row let connect = fun ((model, path):node) cb -> let row = model#get_iter path in - model#set ~row ~column:event cb + let current_cb = try model#get ~row ~column:event with _ -> fun _ -> () in + model#set ~row ~column:event (fun e -> cb e; current_cb e) let tree_menu_popup = fun dtd (model:GTree.tree_store) (row:Gtk.tree_iter) -> let menu = GMenu.menu () in @@ -443,4 +472,4 @@ let create = fun dtd xml -> let _ = tree_view#drag#connect#motion ~callback:motion in let _ = tree_view#drag#connect#drop ~callback:drop in - tree_model, window + (tree_model, tree_view), window diff --git a/sw/lib/ocaml/xmlEdit.mli b/sw/lib/ocaml/xmlEdit.mli index 88940327f2..fdb814f53a 100644 --- a/sw/lib/ocaml/xmlEdit.mli +++ b/sw/lib/ocaml/xmlEdit.mli @@ -35,7 +35,8 @@ node designation: it may not remain valid after strucure modifications (reordering, deletion addition, ... *) type tag = string -type attributes = (string * string) list +type attribute = string * string +type attributes = attribute list type event = Deleted | Modified of attributes | New_child of node @@ -54,8 +55,10 @@ val tag : node -> string val attribs : node -> attributes val attrib : node -> string -> string (* No case match *) val children : node -> node list +val parent : node -> tag -> node (** May raise Failure *) (** Xml-light like acces functions *) +val set_attrib : node -> attribute -> unit val set_attribs : node -> attributes -> unit val delete : node -> unit val add_child : node -> tag -> attributes -> node @@ -63,3 +66,5 @@ val add_child : node -> tag -> attributes -> node val connect : node -> (event -> unit) -> unit (** To be kept informed about modifications *) + +val selection : t -> node diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index 7510c71af9..c5f221e773 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -307,19 +307,20 @@ let remove_attribs = fun xml names -> let xml_assoc_attrib = fun a v xmls -> List.find (fun x -> ExtXml.attrib x a = v) xmls -let coords_of_waypoint = fun wp -> - (ExtXml.float_attrib wp "x", ExtXml.float_attrib wp "y") +let g2D_of_waypoint = fun wp -> + { G2D.x2D = ExtXml.float_attrib wp "x"; y2D = ExtXml.float_attrib wp "y" } -let coords_of_wp_name = fun wp waypoints -> +let g2D_of_wp_name = fun wp waypoints -> let wp = xml_assoc_attrib "name" wp waypoints in - (ExtXml.float_attrib wp "x", ExtXml.float_attrib wp "y") + g2D_of_waypoint wp let new_waypoint = fun wp qdr dist waypoints -> let wp_xml = xml_assoc_attrib "name" wp !waypoints in - let wpx, wpy = coords_of_waypoint wp_xml in + let wp2D = g2D_of_waypoint wp_xml in let a = (Deg>>Rad)(90. -. qdr) in - let x = string_of_float (wpx +. dist *. cos a) - and y = string_of_float (wpy +. dist *. sin a) in + let xy = G2D.vect_add wp2D (G2D.polar2cart { G2D.r2D = dist; theta2D = a }) in + let x = string_of_float xy.G2D.x2D + and y = string_of_float xy.G2D.y2D in let name = Printf.sprintf "%s_%.0f_%.0f" wp qdr dist in let alt = try ["alt", Xml.attrib wp_xml "alt"] with _ -> [] in waypoints := Xml.Element("waypoint", ["name", name; "x", x; "y", y]@alt, []) :: !waypoints; @@ -394,16 +395,15 @@ let process_relative_waypoints = fun xml -> (** Path preprocessing: a list of waypoints is translated into an alternance of route and circle stages *) -let compile_path = fun wpts radius last_last last ps rest -> +let compile_path = fun wpts default_radius last_last last ps rest -> let rec loop = fun p0 last ps -> match ps with [] -> rest | p::ps -> let wp = Xml.attrib p "wp" in - let (x1, y1) = coords_of_wp_name last wpts - and (x2, y2) = coords_of_wp_name wp wpts in - let p1 = {G2D.x2D=x1; y2D=y1} - and p2 = {G2D.x2D=x2; y2D=y2} in + let p1 = g2D_of_wp_name last wpts + and p2 = g2D_of_wp_name wp wpts in + let radius = try ExtXml.float_attrib p "radius" with _ -> default_radius in let (c, f, s) = G2D.arc_segment p0 p1 p2 radius in (* Angle between P1 and F *) @@ -452,8 +452,7 @@ let stage_process_path = fun wpts stage rest -> "hmode","route"; "wp", wp2], []):: (* Here starts the actual translation *) - let x1, y1 = coords_of_wp_name wp1 wpts in - let p1 = {Geometry_2d.x2D=x1; y2D=y1} in + let p1 = g2D_of_wp_name wp1 wpts in compile_path wpts radius p1 wp2 ps rest else stage::rest