1. bergsoe
  2. papl


papl / src / PaplTrajectory.ml

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

type rng_t = PaplRandom.rng_t

type 'a t = {
  t0 : float;
  t1 : float;
  get : float -> 'a

type range_t = float * float

let sprintf = Printf.sprintf

let fmt_range traj = sprintf "range = (%f, %f)" traj.t0 traj.t1

let range_is_empty (t0, t1) = t1 < t0

let range_is_within (t0, t1) (t0', t1') = t0' <= t0 && t1 <= t1'

let range_intersect (t0, t1) (t0', t1') = max t0 t0', min t1 t1'

let make (t0, t1) get = {
  t0 = t0;
  t1 = t1;
  get = get;

let empty () = make (0., -1.) (fun _ -> failwith "Internal error: empty.")

let make_fixed range x = make range (BatPervasives.const x)

let in_range traj t = traj.t0 <= t && t <= traj.t1

let is_empty traj = traj.t1 < traj.t0

let is_zero traj = traj.t0 = traj.t1

let get traj t =
  if in_range traj t then
    traj.get t
         "PaplTrajectory.get: t out of range. (t = %f, %s)"
         t (fmt_range traj))

let t0 traj = traj.t0

let t1 traj = traj.t1

let range traj = (traj.t0, traj.t1)
let get_range = range

let x0 traj = get traj (t0 traj)

let x1 traj = get traj (t1 traj)

let duration traj = traj.t1 -. traj.t0

let shift { t0; t1; get } dt =
  make (t0 +. dt, t1 +. dt) (fun t -> get (t -. dt))

let shift_to traj t0_new = shift traj (t0_new -. traj.t0)

let shift_t1_to traj t1_new = shift traj (t1_new -. traj.t1)

let scale { t0; t1; get } s =
  if s < 0.
  then invalid_arg
    (sprintf "PaplTrajectory.scale: s < 0. (s = %f)" s)
  else if s = 0.
  then make_fixed (t0, t0) (get t1)
  else make
    (t0, t0 +. (t1 -. t0) *. s)
    (fun t -> get ((t -. t0) /. s +. t0))

let scale_to traj d =
  let d' = duration traj in
    if d' < 0. then invalid_arg
      (sprintf "PaplTrajectory.scale_to: duration < 0 (%s)" (fmt_range traj))
      if d' = 0.
      then let t0 = t0 traj in make_fixed (t0, t0 +. d) (x1 traj)
      else scale traj (d /. d')

let of_interpolate (t0, t1) ip =
  if t1 < t0 then invalid_arg "PaplTrajectory.of_interpolate: t1 < t0";
  shift_to (scale { t0 = 0.; t1 = 1.; get = ip } (t1 -. t0)) t0

let of_metric ?(t0 = 0.) interpolate metric a b =
  let t1 = t0 +. metric a b in
    if t1 < t0 then invalid_arg "PaplTrajectory.of_metric: t1 < t0";
    of_interpolate (t0, t1) (interpolate a b)

let map f { t0; t1; get } = make (t0, t1) (fun t -> f (get t))

let of_list ?(t0 = 0.) trajectories =
  let () =
    match trajectories with
        [] -> invalid_arg "PaplTrajectory.of_list: empty list"
      | _ -> ()
  let module Ord = struct
    type t = float * float
    let compare (a0, a1) (b0, b1) =
      if (a0 = a1 && b0 <= a0 && a0 <= b1) ||
        (b0 = b1 && a0 <= b0 && b0 <= a1) then 0 (* If overlap. *)
      else if a0 < b0 then -1
      else 1
  let module Map = BatMap.Make (Ord) in
  let (t1, table) =
      (fun (t_prev, table) traj ->
         if is_empty traj then invalid_arg
           (sprintf "PaplTrajectory.of_list: empty trajectory (%s)"
              (fmt_range traj))
         else if is_zero traj then (* Skip zero-length trajectories. *)
           (t_prev, table)
           let t0, t1 = traj.t0, traj.t1 in
           let len = t1 -. t0 in
           let t_next = t_prev +. len in
           let key = (t_prev, t_next) in
           let table =
             Map.add key (shift_to traj t_prev) table
             (t_next, table))
      (t0, Map.empty)
    if t0 = t1 then
      let x1 = x1 (BatList.last trajectories) in
        make_fixed (t0, t1) x1
      let get t = get (Map.find (t, t) table) t
      in make (t0, t1) get

let of_path ?t0 interpolate metric path =
  of_list ?t0
       (fun x0 x1 -> of_metric interpolate metric x0 x1)

let place small large =
  if small.t0 > large.t1 || small.t1 < large.t0 then
    invalid_arg "PaplTrajectory.place: no overlap between small and large."
    let t0 = min small.t0 large.t0 in
    let t1 = max small.t1 large.t1 in
    let get t =
      if in_range small t
      then get small t
      else get large t
      make (t0, t1) get

let crop range traj =
  if range_is_empty range then
    invalid_arg "PaplTrajectory.crop: range is empty."
  else if not (range_is_within range (get_range traj)) then
    invalid_arg "PaplTrajectory.crop: range is not subrange of traj."
    make range traj.get

let intersect_crop range traj =
  make (range_intersect range (get_range traj)) traj.get

let repeat traj =
  if is_empty traj then
    invalid_arg "PaplTrajectory.repeat: trajectory is empty."
    let len = duration traj in
    let t0 = traj.t0 in
    let range = (t0, infinity) in
      if len = 0. then make_fixed range (x1 traj)
        let get t = get traj (t0 +. mod_float (t -. t0) len) in
          make range get

let repeat_n n traj =
  let t0 = traj.t0 in
  if n < 0 then invalid_arg "PaplTrajectory.repeat_n: n < 0"
  else if n = 0 then make_fixed (t0, t0) (x1 traj)
  else if n = 1 then traj
    let t1 = t0 +. float_of_int n *. duration traj in
        (make_fixed (t1, t1) (x1 traj)) (* Correct the end point. *)
        (crop (t0, t1) (repeat traj))

let rev traj =
  let sum = traj.t0 +. traj.t1 in
    make (range traj) (fun t -> get traj (sum -. t))

let range_to t1 = (neg_infinity, t1)

let range_from t0 = (t0, infinity)

let range_all = (neg_infinity, infinity)

let find_discontinuity traj dt (metric, eps) =
  let bad x0 x1 = metric x0 x1 > eps in
  let rec loop (t_prev, x_prev) =
    let t_next = t_prev +. dt in
    if t_next > traj.t1 then
      if bad x_prev (x1 traj) then Some t_prev
      else None
      let x_next = get traj t_next in
        if bad x_prev x_next then Some t_prev
        else loop (t_next, x_next)
    loop (traj.t0, x0 traj)

module Sampler = struct
  let uniform ?rng traj =
    if is_empty traj then BatEnum.empty ()
      let range = range traj in
      let get_uniform = PaplSampler.get_uniform ?rng in
      let next () =
        let t = get_uniform range in
          get traj t
        BatEnum.from next

  let get_uniform ?rng =
    let get_uniform = PaplSampler.get_uniform ?rng in
      fun traj ->
        let t = get_uniform (range traj) in
          get traj t

module Tuple2 = struct
  let combine (ta, tb) =
    let t0 = max (t0 ta) (t0 tb) in
    let t1 = min (t1 ta) (t1 tb) in
    let get t = (get ta t, get tb t) in
      make (t0, t1) get

module Tuple3 = struct
  let combine (ta, tb, tc) =
    let t0 = max (max (t0 ta) (t0 tb)) (t0 tc) in
    let t1 = min (min (t1 ta) (t1 tb)) (t1 tc) in
    let get t = (get ta t, get tb t, get tc t) in
      make (t0, t1) get

module Array = struct
  let combine trajectories =
    let t0s = Array.map t0 trajectories in
    let t1s = Array.map t1 trajectories in
    let t0 = BatArray.max t0s in
    let t1 = BatArray.min t1s in
    let get t =
        (fun traj -> get traj t)
      make (t0, t1) get