Source

papl / src / PaplRRTExpand.ml

Full commit
(*
  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
*)

type 'a expand_t = 'a PaplRRT.expand_t

let by_intermediaries ?(accept_first = false) constr intermediaries = ();
  let accept edge = PaplConstraint.accept constr edge in
    fun start goal ->
      let rec advance prev path stream =
        match BatEnum.get stream with
          | None ->
              let reached = accept (prev, goal) in
              let path = if reached then goal :: path else path in
                (prev, path, reached)
          | Some next ->
              if accept (prev, next) then
                advance next (next :: path) stream
              else
                (prev, path, false)
      in
      let rec loop its (prev, path, reached) =
        let result () = (List.rev path, reached) in
          match its, reached with
              _, true | [], _ -> result ()
            | it :: its, false ->
                if accept_first && not (BatList.is_empty path) then
                  result ()
                else
                  loop its (advance prev path (it prev goal))
      in
        loop intermediaries (start, [], false)

let make_intermediaries ip metric steps =
  List.map
    (fun step ->
       PaplInterpolate.intermediary_steps
         ip (metric, step))
    steps

let by_intermediary_steps ?accept_first constr ip metric steps =
  by_intermediaries
    ?accept_first
    (PaplPlannerConstraint.constrain_edge_and_snd constr)
    (make_intermediaries ip metric steps)

let quot_rem a b = a / b, a mod b

let split_n ?min_stride ?max_stride n path =
  let len = List.length path in
  let min_stride = BatOption.default 0 min_stride in
  let max_stride = BatOption.default len max_stride in
  let shorten_n n =
    if len <= n then path
    else
      let i, r = quot_rem (len - n) n in
      let rec loop r path =
        let skip = if r > 0 then i + 1 else i in
          match BatList.drop skip path with
              [q] -> [q]
            | q :: path -> q :: loop (r - 1) path
            | [] -> failwith "PaplRRTExpand.split_n: impossible"
      in loop r path
  in
    if len = 0 then []
    else if min_stride >= len then [BatList.last path]
    else
      let i, r = quot_rem (len - n) n in
      let n =
        (* If too few elements are dropped: *)
        if i + 1 < min_stride then
          len / min_stride
        (* If too many elements are dropped: *)
        else if i + 1 > max_stride || r > 0 && i + 2 > max_stride then
          len / max_stride + 1
        else
          n
      in shorten_n n

let steps_n ?min_stride ?max_stride constr intermediary n =
  let () =
    if n <= 0 then
      let msg = Printf.sprintf
        "PaplRRTExpand.steps_n: n = %d must be greater than 0."
        n
      in invalid_arg msg
  in
  let expand = by_intermediaries constr [intermediary] in
    fun start goal ->
      let (path, reached) = expand start goal in
        split_n ?min_stride ?max_stride n path, reached

let intermediary_option interpolate (metric, eps) constr =
  let ok a b = PaplConstraint.accept constr (a, b) in
    fun a b ->
      let ip = interpolate a b in
      let step = eps /. metric a b in
      let rec loop prev pos acc =
        if pos >= 1.0 then (List.rev acc, true)
        else
          match ip pos with
              None -> (List.rev acc, false)
            | Some q ->
                if ok prev q
                then loop q (pos +. step) (q :: acc)
                else (List.rev acc, false)
      in loop a step []