module Math.Spline.BSpline.Internal
( BSpline(..)
, mapControlPoints
, evalBSpline
, evalNaturalBSpline
, insertKnot
, deBoor
, slice
) where
import Math.Spline.Knots
import Control.Monad.ST
import Data.Monoid
import qualified Data.Vector.Generic as V
import qualified Data.Vector.Generic.Mutable as MV
import qualified Data.Vector as BV (Vector)
import qualified Data.Vector.Unboxed as UV (Vector, Unbox)
import Data.VectorSpace
import Prelude as P
data BSpline v t = Spline
{ degree :: !Int
, knotVector :: Knots (Scalar t)
, controlPoints :: v t
}
deriving instance (Eq (Scalar a), Eq (v a)) => Eq (BSpline v a)
deriving instance (Ord (Scalar a), Ord (v a)) => Ord (BSpline v a)
instance (Show (Scalar a), Show a, Show (v a)) => Show (BSpline v a) where
showsPrec p (Spline _ kts cps) = showParen (p>10)
( showString "bSpline "
. showsPrec 11 kts
. showChar ' '
. showsPrec 11 cps
)
mapControlPoints :: (Scalar a ~ Scalar b, V.Vector v a, V.Vector v b) => (a -> b) -> BSpline v a -> BSpline v b
mapControlPoints f spline = spline
{ controlPoints = V.map f (controlPoints spline)
, knotVector = knotVector spline
}
evalBSpline :: ( VectorSpace a, Fractional (Scalar a), Ord (Scalar a)
, V.Vector v a, V.Vector v (Scalar a))
=> BSpline v a -> Scalar a -> a
evalBSpline spline
| V.null (controlPoints spline) = zeroV
| otherwise = V.head . P.last . deBoor spline
evalNaturalBSpline :: (Fractional (Scalar a), Ord (Scalar a), VectorSpace a, V.Vector v a)
=> BSpline v a -> Scalar a -> a
evalNaturalBSpline spline x = runST $ do
let Spline p kvec cps = slice spline x
kts = V.tail (knotsVector kvec)
s = p 1 V.length (V.takeWhile (x==) kts)
ds <- V.thaw cps
sequence_
[ do
let !u0 = kts V.! (j + i)
!u1 = kts V.! (j + p)
!du = u1 u0
!a = if du <= 0 then 1 else (x u0) / du
d0 <- MV.read ds j
d1 <- MV.read ds (j + 1)
MV.write ds j (lerp d0 d1 a)
| i <- [0 .. s]
, j <- [0 .. s i]
]
MV.read ds 0
insertKnot
:: (VectorSpace a, Ord (Scalar a), Fractional (Scalar a), V.Vector v a, V.Vector v (Scalar a)) =>
BSpline v a -> Scalar a -> BSpline v a
insertKnot spline x = spline
{ knotVector = knotVector spline `mappend` knot x
, controlPoints = V.zipWith4 (interp x) us (V.drop p us) ds (V.tail ds)
}
where
us = V.convert $ knotsVector (knotVector spline)
p = degree spline
ds = extend (controlPoints spline)
extend :: V.Vector v t => v t -> v t
extend vec
| V.null vec = V.empty
| otherwise = V.cons (V.head vec) (V.snoc vec (V.last vec))
deBoor :: (Fractional (Scalar a), Ord (Scalar a), VectorSpace a, V.Vector v a, V.Vector v (Scalar a))
=> BSpline v a -> Scalar a -> [v a]
deBoor spline x = go us0 (controlPoints spline)
where
us0 = V.convert $ knotsVector (knotVector spline)
uHi = V.drop (degree spline + 1) us0
go us ds
| V.null ds = []
| otherwise = ds : go uLo ds'
where
uLo = V.tail us
ds' = V.zipWith4 (interp x) uLo uHi
ds (V.tail ds)
interp :: (Fractional (Scalar v), Ord (Scalar v), VectorSpace v)
=> Scalar v -> Scalar v -> Scalar v -> v -> v -> v
interp x x0 x1 y0 y1
| x < x0 = y0
| x >= x1 = y1
| otherwise = lerp y0 y1 a
where
a = (x x0) / (x1 x0)
slice :: (Num (Scalar a), Ord (Scalar a), AdditiveGroup a, V.Vector v a)
=> BSpline v a -> Scalar a -> BSpline v a
slice spline x = spline
{ knotVector = stakeKnots (n + n) . sdropKnots (l n) $ knotVector spline
, controlPoints = vtake n . vdrop (l n) $ controlPoints spline
}
where
l = maybe 0 id $ V.findIndex (> x) us
n = degree spline + 1
us = knotsVector (knotVector spline)
vtake :: (V.Vector v t, AdditiveGroup t) => Int -> v t -> v t
vtake n xs
| n <= V.length xs = V.take n xs
| otherwise = xs V.++ V.replicate (n V.length xs) zeroV
vdrop :: (V.Vector v t, AdditiveGroup t) => Int -> v t -> v t
vdrop n xs
| n >= 0 = V.drop n xs
| otherwise = V.replicate (n) zeroV V.++ xs
stakeKnots :: (Num k, Ord k) => Int -> Knots k -> Knots k
stakeKnots n kts
| n <= nKts = takeKnots n kts
| otherwise = case maxKnot kts of
Nothing -> multipleKnot 0 (n nKts)
Just (k, m) -> setKnotMultiplicity k (m + n nKts) kts
where nKts = numKnots kts
sdropKnots :: (Num k, Ord k) => Int -> Knots k -> Knots k
sdropKnots n kts
| n >= 0 = dropKnots n kts
| otherwise = case minKnot kts of
Nothing -> multipleKnot 0 (n)
Just (k, m) -> setKnotMultiplicity k (m n) kts