Commits

bergsoe committed 1472c99

Expansion constructor function.

Comments (0)

Files changed (2)

 
 type callback_t = unit -> unit
 
+type 'a expand_t = 'a -> 'a -> 'a list * bool
+
 type 'a status_t =
     Advanced of bool * 'a
   | Blocked
       (MakeRRTSetupBase(SA))
       (MakeRRTSetupBase(SB))
 end
+
+let expand_by_intermediaries 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 (prev :: path) stream
+              else
+                (prev, path, false)
+      in
+      let rec loop its (prev, path, reached) =
+        match its, reached with
+            _, true | [], _ ->
+              (List.rev path, reached)
+          | it :: its, false ->
+              loop its (advance prev path (it prev goal))
+      in
+        loop intermediaries (start, [], false)
 
 type callback_t = unit -> unit
 
+type 'a expand_t = 'a -> 'a -> 'a list * bool
+
 module type BASE = sig
   type q_t
   type value_t
 
   module Index : PaplTreeIndex.S_RRT with type q_t = value_t
 
-  val expand : q_t -> q_t -> q_t list * bool
+  val expand : q_t expand_t
   val sampler : q_t option PaplSampler.t
 
   val make_value : q_t -> value_t
     type q_t
     include PaplTreeIndex.BruteForce.RRT.SETUP with type q_t := q_t
 
-    val expand : q_t -> q_t -> q_t list * bool
+    val expand : q_t expand_t
     val sampler : q_t option PaplSampler.t
   end
 
     PaplPlanner.SAMPLER_BIDIR
     with type q_t = SA.q_t
 end
+
+val expand_by_intermediaries :
+  'a PaplEdgeConstraint.t ->
+  'a PaplInterpolate.intermediary_t list ->
+  'a expand_t