From 15bb6ec5ce4d962538b04af365b3d41e67132279 Mon Sep 17 00:00:00 2001 From: "Yves Bertot (he)" Date: Tue, 23 Apr 2024 15:23:38 +0200 Subject: [PATCH] when turning around in a cell, avoid aiming at the cell center, if possible --- theories/generic_trajectories.v | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/theories/generic_trajectories.v b/theories/generic_trajectories.v index f5a6c86..22d1746 100644 --- a/theories/generic_trajectories.v +++ b/theories/generic_trajectories.v @@ -664,6 +664,22 @@ end. Definition path_reverse (s : seq (annotated_point * annotated_point)) := List.map (fun p => (snd p, fst p)) (List.rev_append s nil). +Definition strict_inside_closed p c := + negb (point_under_edge p (low c)) && + point_strictly_under_edge p (high c) && + (R_ltb (left_limit c) (p_x p) && + (R_ltb (p_x p) (right_limit c))). + +Definition safe_intermediate_point_in_cell (p1 p2 : pt) (c : cell) + (ci : nat) := + let new_x := p_x (cell_center c) in + let new_y := R_div (R_add (p_y p1) (p_y p2)) R2 in + let new_pt := {|p_x := new_x; p_y := new_y|} in + if strict_inside_closed new_pt c then + Apt new_pt (ci :: nil) + else + Apt (cell_center c) (ci :: nil). + (* This function creates a safe path from the door between c1 and c2 and the door between c2 and c3. When op1 and op2 are not provided, midpoints are used as path anchors, @@ -691,8 +707,7 @@ let p2 := match op2 with end end in if R_eqb (p_x p1) (p_x p2) then - let intermediate_point := - Apt (cell_center c2) (c2i :: nil) in + let intermediate_point := safe_intermediate_point_in_cell p1 p2 c2 c2i in (Apt p1 (c1i :: c2i :: nil), intermediate_point) :: (intermediate_point, Apt p2 (c2i :: c3i :: nil)) :: nil else @@ -739,12 +754,6 @@ Definition path_adjacent_cells (cells : seq cell) (source target : pt) | None => None end. -Definition strict_inside_closed p c := - negb (point_under_edge p (low c)) && - point_strictly_under_edge p (high c) && - (R_ltb (left_limit c) (p_x p) && - (R_ltb (p_x p) (right_limit c))). - (* find_origin_cells returns a list of cell indices. *) (* If the list is empty, it should mean that the point is not in the safe part of the work space (it is either outside the box or on