path editing

conf/flight_plans/flight_plan.dtd

@@ -119,7 +119,8 @@ approaching_time CDATA #IMPLIED
 gaz CDATA #IMPLIED>
 
 <!ATTLIST path_point
-wp CDATA #REQUIRED>
+wp CDATA #REQUIRED
+radius CDATA #IMPLIED>
 
 <!ATTLIST set
 var CDATA #REQUIRED

sw/ground_segment/cockpit/map2d.ml

@@ -627,52 +627,93 @@ module Edit = struct
 	    dialog#show ()
 
   let radius = ref 30.
-  let path = ref []
+  let path = ref (ref [])
   let cur_arc = ref None
   and cur_seg = ref None
   and cur_f = ref { G2D.x2D = 0.; y2D = 0. }
+
+  let set_segment = fun s p1 p2 ->
+    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 *)
     

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

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) | _ -> ())

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

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

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

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