Commits

bergsoe  committed fcce6e5

Path module for operations on lists of time-stamped values.

  • Participants
  • Parent commits 6231967

Comments (0)

Files changed (2)

File src/PaplTime.ml

 
 let make time q = { time = time; q = q; }
 
+let map_q f qt = make qt.time (f qt.q)
+
+let map_time f qt = make (f qt.time) qt.q
+
 let get_range (qa, qb) = qa.time, qb.time
 let in_range (a, b) t = a <= t && t <= b
 
 let accept_within_time dist_time (x, y) =
   not (reject_within_time dist_time (x, y))
 
+module Path = struct
+  let of_metric ?(t0 = 0.) qs metric =
+    let rec loop t qs =
+      match qs with
+        | [] -> invalid_arg "PaplTime.Path.of_metric: empty path."
+        | [a] -> [make t a]
+        | a :: b :: rest ->
+            let d = metric a b in
+              make t a :: loop (t +. d) (b :: rest)
+    in
+      loop t0 qs
+
+  let scale_to path len =
+    let x0 = BatList.first path in
+    let x1 = BatList.last path in
+    let t0 = x0.time in
+    let t1 = x1.time in
+    let d = t1 -. t0 in
+      if d = 0.
+      then [x0; make len x1.q]
+      else
+        let scale = len /. d in
+          List.map
+            (fun qt ->
+               let t = qt.time in
+               let t' = t0 +. scale *. (t -. t0) in
+                 make t' qt.q)
+            path
+end
+
 module Trajectory = struct
   let of_interpolate interpolate a b =
     let ip = interpolate a b in

File src/PaplTime.mli

     [make time q] is equivalent to [{ time = time; q = q; }]
 *)
 
+(** {2 Operations} *)
+
+val map_q : ('a -> 'b) -> 'a t -> 'b t
+
+val map_time : (float -> float) -> 'a t -> 'a t
+
 (** {2 Planning spaces}
 *)
 
   val get_uniform : ?rng:rng_t -> range_t -> 'a PaplSampler.t -> 'a t option
 end
 
+(** {2 Paths} *)
+
+module Path : sig
+  val of_metric : ?t0:float -> 'a list -> 'a PaplMetric.t -> 'a t list
+
+  val scale_to : 'a t list -> float -> 'a t list
+  (* [scale_to path len] scales the time stamps of [path] such that the duration
+     of the path is equal to [len].
+
+     If the duration of [path] is zero, then [path] should be of length 2.
+  *)
+end
+
 (** {2 Trajectories} *)
 
 module Trajectory : sig