Source

papl / src / PaplStraightPlanner.ml

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

let re = PaplConstraint.reject

let point_planner (nc, ec) =
  let planner stop (start, goal) =
    if PaplStopCriteria.stop stop then
      PaplPlanner.fail "Stop criteria says stop"
    else if re nc start then
      PaplPlanner.fail "Start point rejected"
    else if re nc goal then
      PaplPlanner.fail "Goal point rejected"
    else if re ec (start, goal) then
      PaplPlanner.fail "Start to goal edge rejected"
    else
      [start; goal]
  in
    planner

let dummy_point_planner stop (a, b) = [a; b]

let dummy_region_planner stop (a, b) =
  let rec loop s =
    if PaplStopCriteria.stop stop
    then PaplPlanner.fail "Stopping criteria says stop."
    else
      match BatEnum.get s with
        | Some (x :: _) -> x
        | Some [] -> loop s
        | None -> PaplPlanner.fail "Region is empty."
  in
    [loop a; loop b]