Source

papl / src / PaplQ.ml

(*
  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 *)

let interpolate_circular n circular =
  let interpolators = Array.make n PaplInterpolate.Float.interpolate in
  let () =
    List.iter
      (fun i -> interpolators.(i) <-
         fun a b ->
           let open PaplTransform.SO2 in
           let ip = interpolate (rotate a) (rotate b) in
             fun s -> angle (ip s))
      circular
  in
  (* 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 a b ->
      let ips = Array.mapi
        (fun i interpolate -> interpolate a.(i) b.(i))
        interpolators
      in
        fun s -> Array.map (fun ip -> ip s) ips