Source

papl / src / PaplQ.ml

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

(* module Sampler = PaplSampler *)

include PaplVector.Array

let ( *=:) a s = Array.iteri (fun i ai -> a.(i) <- ai *. s) a

let (/=:) a s = Array.iteri (fun i ai -> a.(i) <- ai /. s) a

let (+=:) a b = BatArray.iter2i (fun i ai bi -> a.(i) <- ai +. bi) a b

let (-=:) a b = BatArray.iter2i (fun i ai bi -> a.(i) <- ai -. bi) a b

let with_size f (lower, upper) = f (upper -: lower)

let dist_inf_box_normalized = with_size dist_inf_size_normalized

let dist2_box_normalized = with_size dist2_size_normalized

let dist2_sqr_box_normalized = with_size dist2_sqr_size_normalized

(* Boxes *)

module Box = struct
  type box_t = t * t

  let scale s (a, b) = (s *: a, s *: b)
  let translate q (a, b) = (a +: q, b +: q)

  let intersect (a0, a1) (b0, b1) = (max a0 b0, min a1 b1)

  let is_empty (a, b) =
    let n = Array.length a in
    let rec loop i =
      if i >= n then false
      else if a.(i) > b.(i) then
        true
      else
        loop (i + 1)
    in loop 0

  let center (a, b) =
    let d = (b -: a) /: 2.0
    in fun q -> (q -: d, q +: d)

  let within (a, b) q =
    let n = Array.length q in
    let ok i = a.(i) <= q.(i) && q.(i) <= b.(i) in
    let rec loop i =
      if i = n then true
      else if ok i then loop (i + 1)
      else false
    in
      loop 0

  let within_by_eps eps (a, b) q =
    let n = Array.length q in
    let ok i = a.(i) -. eps <= q.(i) && q.(i) <= b.(i) +. eps in
    let rec loop i =
      if i = n then true
      else if ok i then loop (i + 1)
      else false
    in
      loop 0
end

(* Gram-Schmidt *)

let gram_schmidt_by dot n make =
  let norm a = Pervasives.sqrt (dot a a) in
  let eps = 1e-12 in
  let rec loop j basis =
    if j == n then basis
    else begin
      let v = make () in
        List.iter (fun u -> v -=: proj_unit_by dot v u) basis;
        let len = norm v in
          if len < eps then
            (* Try again if no new basis vector. *)
            loop j basis
          else begin
            (* Normalize and create the next vector. *)
            v /=: len;
            loop (j + 1) (v :: basis)
          end
    end
  in loop 0 []

let gram_schmidt n make = gram_schmidt_by dot n make

(* Circular joints *)

type my_t = t

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
    let () = List.iter
      (fun i -> buf.(i) <- angle_interpolate)
      circular
    in
      buf

  let dist_array =
    let buf = Array.make n (fun a b -> a -. b) in
    let () = List.iter
      (fun i -> buf.(i) <- angle_dist)
      circular
    in
      buf

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

  let interpolate a b =
    (* 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. *)
    let ips = Array.mapi
      (fun i interpolate -> interpolate a.(i) b.(i))
      interpolate_array
    in
      fun s -> Array.map (fun ip -> ip s) ips

  module MetricOp = struct
    type t = my_t
    type weight_t = t

    let dim_weight _ = n

    let map_weight = Array.map

    let combine1 f g xs =
      let v i = f xs.(i) in
      let sum = ref (v 0) in
        for i = 1 to n - 1 do
          sum := g !sum (v i)
        done;
        !sum

    let combine2 f g xs ys =
      let v i = f xs.(i) ys.(i) in
      let sum = ref (v 0) in
        for i = 1 to n - 1 do
          sum := g !sum (v i)
        done;
        !sum

    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
      let sum = ref (v 0) in
        for i = 1 to n - 1 do
          sum := outer !sum (v i)
        done;
        !sum

    let combine_weight_x_diff inner outer ws xs ys =
      let v i = inner (ws.(i) *. dist i xs ys) in
      let sum = ref (v 0) in
        for i = 1 to n - 1 do
          sum := outer !sum (v i)
        done;
        !sum
  end

  include PaplVectorMetric.Make (MetricOp)
end