Commits

bergsoe  committed 19a79cb

[relative_speeds] time expansion.

  • Participants
  • Parent commits 46ce1b1

Comments (0)

Files changed (2)

File src/PaplSBLExpand.ml

           else loop ()
   in loop ()
 
-let random_range ?rng =
-  let float_fun = PaplRandom.float ?rng in
-    fun (a, b) -> a +. float_fun (b -. a)
+let random_range = PaplSampler.get_uniform
 
 let stop () = raise BatEnum.No_more_elements
 
   type 'a t = 'a PaplTime.t
 
   let make expand_time expand_q =
-    let make qt =
-      let time_sampler = expand_time qt.PaplTime.time in
-      let q_sampler = expand_q qt.PaplTime.q in
-        BatEnum.map
-          (fun (t, q) -> PaplTime.make t q)
-          (BatEnum.combine (time_sampler, q_sampler))
-          (* Why doesn't BatEnum have a [map2]? *)
-    in
-      make
+    let expand qt =
+      let open PaplTime in
+        PaplSampler.map2
+          (fun t q -> PaplTime.make t q)
+          (expand_time qt.time)
+          (expand_q qt.q)
+    in expand
 
   let scale_offset expand interpolate dist_time =
     let dist_time = PaplTime.Metric.q dist_time in
         BatEnum.map adjust (expand qat)
     in
       make
+
+  let relative_speeds speeds q_time_dist expand_q =
+    let expand qt =
+      let open PaplTime in
+        PaplSampler.map2
+          (fun speed q ->
+             let dt = q_time_dist qt.q q in
+             let t = qt.time +. dt /. speed in
+               PaplTime.make t q)
+          (speeds qt)
+          (expand_q qt.q)
+    in expand
 end
 
 module RRT = struct
   type 'a expand_sampler_t = 'a -> 'a -> 'a PaplSampler.t
   type 'a expand_t = 'a -> 'a -> 'a option
 
-  let random_range ?rng =
-    let float_fun = PaplRandom.float ?rng in
-      fun (a, b) -> a +. float_fun (b -. a)
-
   let to_expand constr expansions = ();
     fun qa qb ->
       BatEnum.Exceptionless.find

File src/PaplSBLExpand.mli

     to the traversal time according to [dist_time] then [q'] is returned.
     Otherwise the time value of [q'] is set according to the traversal time.
 *)
+
+  val relative_speeds :
+    ('a t -> float PaplSampler.t) ->
+    ('a PaplMetric.t) ->
+    'a expand_sampler_t ->
+    'a t expand_sampler_t
 end
 
 module RRT : sig