anb.ml

   1 open Format
   2
   3 let quat0 posecount sbuf =
   4   let i = Xff.rfloat sbuf 0
   5   and j = Xff.rfloat sbuf 4
   6   and k = Xff.rfloat sbuf 8
   7   and s = Xff.rfloat sbuf 12 in
   8   let q = Qtr.make i j k s in
   9   Array.create posecount q
  10 ;;
  11
  12 let quat3456 typ posecount sectbuf sbuf =
  13   let floats = Array.init 8 (fun i -> Xff.rfloat sbuf (i*4)) in
  14   let offset = Xff.rint sbuf 32 in
  15   let sbuf32 = Xff.sbufplus sectbuf offset in
  16   let int32s = Array.init posecount (fun i -> Xff.r32 sbuf32 (i*4)) in
  17   let toquat poseno =
  18     let cq = int32s.(poseno) in
  19     let omsqrt a b c bitno =
  20       let v = 1.0 -. (a*.a +. b*.b +. c*.c) in
  21       if v > 1e-9
  22       then
  23         let r = sqrt v in
  24         if Int32.to_int (Int32.shift_right_logical cq bitno) land 1 = 1
  25         then -.r
  26         else r
  27       else
  28         0.0
  29     in
  30     let nbf shift n =
  31       let i = 511 land Int32.to_int (Int32.shift_right_logical cq shift) in
  32       let i = i - ((i land 0b100000000) lsl 1) in
  33       let f = float i /. 256.0 in
  34       f*.floats.(n*2 + 1) +. floats.(n*2)
  35     in
  36     let i, j, k, s =
  37       match typ with
  38       | 3 ->
  39           let j = nbf 18 0
  40           and k = nbf  9 1
  41           and s = nbf  0 2 in
  42           (omsqrt j k s 31, j, k, s)
  43       | 4 ->
  44           let i = nbf 23 0
  45           and k = nbf  9 1
  46           and s = nbf  0 2 in
  47           (i, omsqrt i k s 22, k, s)
  48       | 5 ->
  49           let i = nbf 23 0
  50           and j = nbf 14 1
  51           and s = nbf  0 2 in
  52           (i, j, omsqrt i j s 13, s)
  53       | 6 ->
  54           let i = nbf 23 0
  55           and j = nbf 14 1
  56           and k = nbf  5 2 in
  57           (i, j, k, omsqrt i j k 4)
  58       | _ -> failwith "Me fail english? That's Umpossible!"
  59     in
  60     Qtr.make i j k s
  61   in
  62   Array.init posecount toquat
  63 ;;
  64
  65 let quat12 posecount sectbuf sbuf =
  66   let floats = Array.init 6 (fun i -> Xff.rfloat sbuf (i*4)) in
  67   let offset = Xff.rint sbuf 24 in
  68   let sbuf16 = Xff.sbufplus sectbuf offset in
  69   let madd v n =
  70     float v /. 32768.0 *. floats.(n*2 + 1) +. floats.(n*2)
  71   in
  72   let toquat poseno =
  73     let a = Xff.r16s sbuf16 (poseno*6 + 0)
  74     and b = Xff.r16s sbuf16 (poseno*6 + 2)
  75     and c = Xff.r16s sbuf16 (poseno*6 + 4) in
  76     let sign = a land 1 = 1 in
  77     let i = madd a 0
  78     and j = madd b 1
  79     and k = madd c 2 in
  80     let m = 1.0 -. (i*.i +. j*.j +. k*.k) in
  81     let s = if m > 1e-9 then sqrt m else 0.0 in
  82     Qtr.make i j k (if sign then -.s else s)
  83   in
  84   Array.init posecount toquat
  85 ;;
  86
  87 let quat13 posecount sectbuf sbuf =
  88   let bias   = Xff.rfloat sbuf 0
  89   and scale  = Xff.rfloat sbuf 4
  90   and x      = Xff.rfloat sbuf 8
  91   and y      = Xff.rfloat sbuf 12
  92   and z      = Xff.rfloat sbuf 16 in
  93   let offset = Xff.rint   sbuf 20 in
  94   let sbuf16 = Xff.sbufplus sectbuf offset in
  95   let toquat poseno =
  96     let fixp = Xff.r16s sbuf16 (poseno*2) in
  97     let fltp = float fixp /. 32768.0 in
  98     let phi = fltp*.scale +. bias in
  99     let q = Qtr.from_axis_angle x y z phi in
 100     { q with Qtr.s = q.Qtr.s }
 101   in
 102   Array.init posecount toquat
 103 ;;
 104
 105 let exact bones poseno =
 106   Array.map (fun a -> Array.get a poseno) bones
 107 ;;
 108
 109 let interpolated bones sposeno dposeno t =
 110   Array.init (Array.length bones)
 111     (fun i ->
 112       let sq = bones.(i).(sposeno)
 113       and dq = bones.(i).(dposeno) in
 114       Qtr.slerp sq dq t
 115     )
 116 ;;
 117
 118 let r xff sbufxff =
 119   if Array.length xff.Xff.sections != 2
 120   then
 121     Xff.sbuferr sbufxff 0 "number of xff sections is not 2"
 122   ;
 123   let sectpos = xff.Xff.sections.(1).Xff.off in
 124   let sectbuf = Xff.sbufplus sbufxff sectpos in
 125   let anbbuf = Xff.sbufplus sectbuf xff.Xff.entry in
 126   if not (Xff.check32 anbbuf 0 12l)
 127   then
 128     Xff.sbuferr anbbuf 0 "bad anb signature"
 129   ;
 130
 131   let bonecount = Xff.rint anbbuf 20 in
 132   let boneD =
 133     let off = Xff.rint anbbuf 16 in
 134     Array.init bonecount (fun i -> Xff.rint sectbuf (off + i*4))
 135   in
 136   let boneB =
 137     let off = Xff.rint anbbuf 8 in
 138     Array.init bonecount (fun i -> Xff.r8 sectbuf (off + i))
 139   in
 140   let posecount = 0xffff land Xff.rint anbbuf 4 in
 141   let rbone i =
 142     let d = boneD.(i)
 143     and b = boneB.(i) in
 144     let sbuf = Xff.sbufplus sectbuf d in
 145     match b with
 146     | 0             ->    quat0   posecount         sbuf
 147     | 3 | 4 | 5 | 6 -> quat3456 b posecount sectbuf sbuf
 148     | 12            ->   quat12   posecount sectbuf sbuf
 149     | 13            ->   quat13   posecount sectbuf sbuf
 150     | _             -> failwith "Me fail english? That's Umpossible!"
 151   in
 152   posecount, Array.init bonecount rbone
 153 ;;
 154
 155 let append (p1, b1) (p2, b2) =
 156   let b = Array.init (Array.length b1) (fun i -> Array.append b1.(i) b2.(i)) in
 157   p1 + p2, b;
 158 ;;