qtr.ml

   1 (* A lot of this was taken from different sources
   2    Including NVidias examples
   3    Mikael Kalms' code
   4    Wikipedia
   5 *)
   6
   7 open Vec
   8
   9 type t = { i : float; j : float; k : float; s : float }
  10
  11 open FltOp
  12
  13 let id = { i = 0.; j = 0.; k = 0.; s = 1.0 }
  14 let conjugate q = { i = -.q.i; j = -.q.j; k = -.q.k; s = q.s }
  15 let magnitude_squared q = q.i * q.i + q.j * q.j + q.k * q.k + q.s * q.s
  16 let magnitude q = sqrt (magnitude_squared q)
  17 let scale q s = { i = q.i * s; j = q.j * s; k = q.k * s; s = q.s * s }
  18 let norm q = let s = 1.0 / magnitude q in scale q s
  19 let add q p = { i = q.i + p.i; j = q.j + p.j; k = q.k + p.k; s = q.s + p.s }
  20 let dot q p = q.i * p.i + q.j * p.j + q.k * p.k + q.s * p.s
  21 let elems q = q.i, q.j, q.k, q.s
  22 let make i j k s = { i = i; j = j; k = k; s = s }
  23
  24 let compose q p =
  25   let s = q.s * p.s - (q.i * p.i + q.j * p.j + q.k * p.k)
  26   and i = q.s * p.i + p.s * q.i + q.j * p.k - q.k * p.j
  27   and j = q.s * p.j + p.s * q.j + q.k * p.i - q.i * p.k
  28   and k = q.s * p.k + p.s * q.k + q.i * p.j - q.j * p.i in
  29   { i = i; j = j; k = k; s = s }
  30
  31 let from_euler yaw pitch roll =
  32   let half_yaw = yaw / 2.0
  33   and half_pitch = pitch / 2.0
  34   and half_roll = roll / 2.0 in
  35
  36   let cosYaw = cos half_yaw
  37   and cosRoll = cos half_roll
  38   and cosPitch = cos half_pitch
  39   and sinYaw = sin half_yaw
  40   and sinRoll = sin half_roll
  41   and sinPitch = sin half_pitch in
  42
  43   { i = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
  44     j = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
  45     k = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
  46     s = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw }
  47
  48 let from_axis_angle x y z phi =
  49   let phi = phi * 0.5 in
  50   let mag = sqrt (x * x + y * y + z * z) in
  51   let s = sin phi / mag in
  52   let x = x * s and y = y * s and z = z * s in
  53   { i = x; j = y; k = z; s = cos phi }
  54
  55 let to_norm_axis_angle q =
  56   let magsq = q.i * q.i + q.j * q.j + q.k * q.k in
  57   if magsq > 0.0 then
  58     let theta = 2.0 * acos q.s in
  59     let s = 1.0 / sqrt magsq in
  60     { z = q.i * s; y = q.j * s; x = q.k * s }, theta
  61   else x_axis, 0.0
  62
  63 let to_axis_angle q = { z = q.i; y = q.j; x = q.k }, 2.0 * acos q.s
  64
  65 let slerp q p t =
  66   let cosphi = dot q p in
  67   let cosphi, flip = if cosphi < 0.0 then -.cosphi, true else cosphi, false in
  68   if 1.0 - cosphi < 1e-8 then
  69     q
  70   else
  71     let phi = acos cosphi in
  72     let sphi = 1.0 / sin phi in
  73     let k0 = sin ((1.0 - t) * phi) * sphi
  74     and k1 = sin (t * phi) * sphi in
  75     let k1 = if flip then -. k1 else k1 in
  76     let q' = scale q k0 in
  77     let p' = scale p k1 in
  78     add q' p'
  79
  80 let apply q v =
  81   let a = -.q.s in
  82   let b = q.i in
  83   let c = q.j in
  84   let d = q.k in
  85   let v1 = v.Vec.x in
  86   let v2 = v.Vec.y in
  87   let v3 = v.Vec.z in
  88   let t2 =   a*.b in
  89   let t3 =   a*.c in
  90   let t4 =   a*.d in
  91   let t5 =  -.b*.b in
  92   let t6 =   b*.c in
  93   let t7 =   b*.d in
  94   let t8 =  -.c*.c in
  95   let t9 =   c*.d in
  96   let t10 = -.d*.d in
  97   { Vec.x = 2.*.( (t8 +. t10)*.v1 +. (t6 -.  t4)*.v2 +. (t3 +. t7)*.v3 ) +. v1
  98   ;     y = 2.*.( (t4 +.  t6)*.v1 +. (t5 +. t10)*.v2 +. (t9 -. t2)*.v3 ) +. v2
  99   ;     z = 2.*.( (t7 -.  t3)*.v1 +. (t2 +.  t9)*.v2 +. (t5 +. t8)*.v3 ) +. v3
 100   }
 101
 102 let print ppf q =
 103   Format.fprintf ppf "[@[<2>i % f;@ j % f;@ k % f;@ s % f@]]" q.i q.j q.k q.s