(*
  skeg - Sex, Kinematics, Elegance and Glory.
  Copyright (C) 2004 David Baelde and Samuel Mimram.

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA 02111-1307, USA.
*)

(** Skeletons and other basic types.

@author David Baelde, Samuel Mimram *)



(* $Id: skel.ml,v 1.9 2004/05/14 09:48:52 dbaelde Exp $ *)

type point = float * float * float

(** Convention for matrixes: m.(x).(y). *)


type 'a matrix = 'a array array

(** A skel is the matrix of connexions. false means not connected. *)


type skel = bool matrix

type position = point array

(** Conversion from a rough constraint to a precise one. *)

let one_of_three c =
  let a = Array.create (3*(Array.length c)) None in
    for i = 0 to (Array.length c)-1 do
      match c.(i) with
        | None -> ()
        | Some (x,y,z) ->
            a.(3*i+0) <- Some x ;
            a.(3*i+1) <- Some y ;
            a.(3*i+2) <- Some z
    done ;
    a

(** A Constraints.t is a partial movement. Some components of the vector can be None. *)

module Constraints =
struct

  type t = point option array

  let empty n = Array.create n None

  let iter f =
    Array.iter
      (function
         | None -> ()
         | Some x -> f x)

  
  (** Utilities, that only apply to non-None elements. *)


  let fold f =
    Array.fold_left
      (fun v -> function
         | None -> v
         | Some x -> f v x) 

  let map f =
    Array.map
      (function
         | None -> None
         | Some x -> Some (f x))

  
  (** Higher level utilities. *)


  let length c =
    let l = ref 0. in
    let norm (x,y,z) = sqrt (x*.x +. y*.y +. z*.z) in
      iter (fun x -> l := !l +. (norm x)) c ;
      sqrt !l

  let add c d =
    assert (Array.length c = Array.length d) ;
    let a = Array.make (Array.length c) None in
      for i = 1 to (Array.length c)-1 do
        match c.(i), d.(i) with
          | None, _
          | _, None -> assert false
          | (Some (x,y,z)), (Some (m,n,o)) ->
              a.(i) <- Some (x+.m, y+.n, z+.o)
      done ;
      a

  let sub c d =
    assert (Array.length c = Array.length d) ;
    let a = Array.make (Array.length c) None in
      for i = 1 to (Array.length c)-1 do
        match c.(i), d.(i) with
          | None, _
          | _, None -> assert false
          | (Some (x,y,z)), (Some (m,n,o)) ->
              a.(i) <- Some (x-.m, y-.n, z-.o)
      done ;
      a

end

(** High-level skeleton type, where nodes are labelled. Not used. *)


module Skeleton =
struct

  type t =
      (string*((string*float)list)) list

end