{-# LANGUAGE CPP #-}
#define Flt Double
#define VECT_Double

-- | Interpolation of vectors. 
-- Note: we interpolate unit vectors differently from ordinary vectors.

module Data.Vect.Flt.Interpolate where

import Data.Vect.Flt.Base
import Data.Vect.Flt.Util.Dim2 (sinCos',angle2')
import Data.Vect.Flt.Util.Dim3 (rotate3')

class Interpolate v where
  interpolate :: Flt -> v -> v -> v
  
instance Interpolate Flt where
  interpolate t x y = x + t*(y-x)

instance Interpolate Vec2 where interpolate t x y = x &+ t *& (y &- x)
instance Interpolate Vec3 where interpolate t x y = x &+ t *& (y &- x)
instance Interpolate Vec4 where interpolate t x y = x &+ t *& (y &- x)

instance Interpolate Normal2 where
  interpolate t nx ny = sinCos' $ ax + t*adiff where
    ax = angle2' nx
    ay = angle2' ny
    adiff = helper (ay - ax)
    helper d 
      | d < -pi   = d + twopi
      | d >  pi   = d - twopi
      | otherwise = d
    twopi = 2*pi
    
instance Interpolate Normal3 where 
  interpolate t nx ny = 
    if maxAngle < 0.001  -- more or less ad-hoc critical angle
      then mkNormal $ interpolate t x y
      else toNormalUnsafe $ rotate3' (t*maxAngle) (mkNormal axis) x where
    x = fromNormal nx
    y = fromNormal ny
    axis = (x &^ y)
    maxAngle = acos (x &. y)