-let interpolate_circular n circular =

- let interpolators = Array.make n PaplInterpolate.Float.interpolate in

- (fun i -> interpolators.(i) <-

- let open PaplTransform.SO2 in

- let ip = interpolate (rotate a) (rotate b) in

+module MakeCircular (Setup : sig val setup : int * int list end) = struct

+ let n, circular = Setup.setup

+ module SO = PaplTransform.SO2

+ let angle_dist a b = SO.dist (SO.rotate a) (SO.rotate b)

+ let angle_interpolate a b =

+ let ip = SO.interpolate (SO.rotate a) (SO.rotate b) in

+ fun s -> SO.angle (ip s)

+ let interpolate_array =

+ let buf = Array.make n PaplInterpolate.Float.interpolate in

+ (fun i -> buf.(i) <- angle_interpolate)

+ let buf = Array.make n (fun a b -> a -. b) in

+ (fun i -> buf.(i) <- angle_dist)

- (* As all other interpolator constructors, the function assumes that the

- interpolation function will be called repeatedly for the same pair of start

- and goal points [a] and [b]. *)

- (fun i interpolate -> interpolate a.(i) b.(i))

- fun s -> Array.map (fun ip -> ip s) ips

+ let dist i xs ys = dist_array.(i) xs.(i) ys.(i)

+ (* As all other interpolator constructors, the function assumes that the

+ interpolation function will be called repeatedly for the same pair of

+ start and goal points. *)

+ (fun i interpolate -> interpolate a.(i) b.(i))

+ fun s -> Array.map (fun ip -> ip s) ips

+ module MetricOp = struct

+ let map_weight = Array.map

+ let combine2 f g xs ys =

+ let v i = f xs.(i) ys.(i) in

+ let combine_value = combine1

+ let combine_weight_x_value inner outer weight xs =

+ combine2 (fun w x -> inner (w *. x)) outer weight xs

+ let combine_diff inner outer xs ys =

+ let v i = inner (dist i xs ys) in

+ sum := outer !sum (v i)

+ let combine_weight_x_diff inner outer ws xs ys =

+ let v i = inner (ws.(i) *. dist i xs ys) in

+ sum := outer !sum (v i)

+ include PaplVectorMetric.Make (MetricOp)