module Geom2D.CubicBezier.Numeric where
import Data.Vector.Unboxed as V
import Data.Vector.Unboxed.Mutable as MV
import Data.Matrix.Unboxed as M
import qualified Data.Matrix.Generic as G
import qualified Data.Matrix.Unboxed.Mutable as MM
import Control.Monad.ST
import Control.Monad
sign x | x < 0 = 1
| otherwise = 1
quadraticRoot :: Double -> Double -> Double -> [Double]
quadraticRoot a b c
| a == 0 && b == 0 = []
| a == 0 = [c/b]
| otherwise = result
where
d = b*b 4*a*c
q = (b + sign b * sqrt d) / 2
x1 = q/a
x2 = c/q
result | d < 0 = []
| d == 0 = [x1]
| otherwise = [x1, x2]
solveLinear2x2 :: (Eq a, Fractional a) => a -> a -> a -> a -> a -> a -> Maybe (a, a)
solveLinear2x2 a b c d e f =
case det of 0 -> Nothing
_ -> Just ((c * e b * f) / det, (a * f c * d) / det)
where det = d * b a * e
data SparseMatrix a =
SparseMatrix (V.Vector Int)
(V.Vector (Int, Int)) (M.Matrix a)
makeSparse :: Unbox a => Vector Int
-> M.Matrix a
-> SparseMatrix a
makeSparse v m = SparseMatrix v (sparseRanges v vars width) m
where
width = cols m
vars = V.last v + width
sparseRanges :: V.Vector Int -> Int -> Int -> V.Vector (Int, Int)
sparseRanges v vars width = ranges
where
height = V.length v
ranges = V.scanl' nextRange (nextRange (0,0) 0) $
V.enumFromN 1 (vars1)
nextRange (s,e) i = (nextStart s i, nextEnd e i)
nextStart s i
| s >= height = height
| v `V.unsafeIndex` s + width <= i =
nextStart (s+1) i
| otherwise = s
nextEnd e i
| e >= height = height
| v `V.unsafeIndex` e > i = e
| otherwise = nextEnd (e+1) i
lsqMatrix :: (Num a, Unbox a) =>
SparseMatrix a
-> Matrix a
lsqMatrix (SparseMatrix rowStart ranges m)
| V.length rowStart /= height =
error "lsqMatrix: lengths don't match."
| otherwise = M.generate (vars, width) coeff
where
(height, width) = dim m
vars = V.last rowStart + width
overlap (s1,e1) (s2, e2) =
(max s1 s2, min e1 e2)
realIndex (r, c) =
(r, c rowStart `V.unsafeIndex` r)
coeff (r,c) = let
(s, e) | r+c >= vars = (0, 0)
| otherwise =
overlap (ranges `V.unsafeIndex` r) (ranges `V.unsafeIndex` (r+c))
in V.foldl' (\acc i -> acc + m `M.unsafeIndex` realIndex (i, r) *
m `M.unsafeIndex` realIndex (i, r+c)) 0 $
V.enumFromN s (es)
addMatrix :: (Num a, Unbox a) => M.Matrix a -> M.Matrix a -> M.Matrix a
addMatrix = M.zipWith (+)
addVec :: (Num a, Unbox a) => V.Vector a -> V.Vector a -> V.Vector a
addVec = V.zipWith (+)
sparseMulT :: (Num a, Unbox a) =>
V.Vector a
-> SparseMatrix a
-> V.Vector a
sparseMulT v (SparseMatrix rowStart ranges m)
| V.length v /= height =
error "sparseMulT: lengths don't match."
| otherwise = V.generate vars coeff
where (height, width) = dim m
vars | V.null rowStart = 0
| otherwise = V.unsafeLast rowStart + width
realIndex (r, c) =
(r, c rowStart `V.unsafeIndex` r)
coeff i =
let (s, e) = ranges `V.unsafeIndex` i
in V.foldl' (\acc j ->
acc + m `M.unsafeIndex` realIndex (j, i) *
v `V.unsafeIndex` j) 0 $
V.enumFromN s (es)
sparseMul :: (Num a, Unbox a) =>
SparseMatrix a
-> V.Vector a
-> V.Vector a
sparseMul (SparseMatrix rowStart _ranges m) v
| V.length v /= vars =
error "sparseMulT: lengths don't match."
| otherwise = V.generate height coeff
where (height, width) = dim m
vars | V.null rowStart = 0
| otherwise = V.unsafeLast rowStart + width
coeff i = V.sum $ V.zipWith (*)
(V.unsafeSlice (rowStart V.! i) width v)
(G.unsafeTakeRow m i)
decompLDL :: (Fractional a, Unbox a) => M.Matrix a -> M.Matrix a
decompLDL m = runST $ do
m2 <- M.thaw m
let (vars, width) = dim m
V.forM_ (V.enumFromN 0 $ vars1) $
\startr -> do
pivot <- MM.unsafeRead m2 (startr, 0)
V.forM_ (V.enumFromN 1 $ width1) $
\c -> do
el <- MM.unsafeRead m2 (startr, c)
MM.unsafeWrite m2 (startr, c) (el/pivot)
V.forM_ (V.enumFromN 0 $ min (width1) $ varsstartr1) $
\r -> do
r0 <- MM.unsafeRead m2 (startr, r+1)
V.forM_ (V.enumFromN 0 (widthr1)) $
\c -> do r1 <- MM.unsafeRead m2 (startr, r+c+1)
el <- MM.unsafeRead m2 (r+startr+1, c)
MM.unsafeWrite m2 (r+startr+1, c)
(el r0*r1*pivot)
M.unsafeFreeze m2
solveLDL :: (Fractional a, Unbox a) =>
M.Matrix a -> V.Vector a -> V.Vector a
solveLDL m v
| rows m /= V.length v = error "solveLDL: lengths don't match"
| otherwise = runST $ do
let (vars, width) = M.dim m
sol1 <- MV.new vars
V.forM_ (V.enumFromN 0 $ min vars width) $
\i -> do
let vi = v `V.unsafeIndex` i
s <- liftM (V.foldl' () vi) $
V.forM (enumFromN 0 i) $
\j -> liftM ((m `M.unsafeIndex` (j, ij)) *)
(MV.unsafeRead sol1 j)
MV.unsafeWrite sol1 i s
V.forM_ (V.enumFromN width $ vars width) $
\i -> do
let vi = v `V.unsafeIndex` i
s <- liftM (V.foldl' () vi) $
V.forM (enumFromN 1 (width1)) $
\j -> liftM ((m `M.unsafeIndex` (ij, j)) *)
(MV.unsafeRead sol1 $ ij)
MV.unsafeWrite sol1 i s
V.forM_ (V.enumFromN 0 $ min vars width) $
\i -> do
solI <- MV.unsafeRead sol1 (varsi1)
let d = m `M.unsafeIndex` (varsi1, 0)
s <- liftM (V.foldl' () (solI/d)) $
V.forM (enumFromN 0 i) $
\j -> liftM ((m `M.unsafeIndex` (varsi1, j+1)) *)
(MV.unsafeRead sol1 $ varsi+j)
MV.unsafeWrite sol1 (varsi1) s
V.forM_ (V.enumFromN width $ vars width) $
\i -> do
solI <- MV.unsafeRead sol1 (varsi1)
let d = m `M.unsafeIndex` (varsi1, 0)
s <- liftM (V.foldl' () (solI/d)) $
V.forM (enumFromN 0 (width1)) $
\j -> liftM ((m `M.unsafeIndex` (varsi1, j+1)) *)
(MV.unsafeRead sol1 $ varsi+j)
MV.unsafeWrite sol1 (varsi1) s
V.unsafeFreeze sol1
lsqSolve :: (Fractional a, Unbox a) =>
SparseMatrix a
-> V.Vector a
-> V.Vector a
lsqSolve m@(SparseMatrix _ _ m') v
| rows m' /= V.length v = error "lsqSolve: lengths don't match"
| otherwise = solveLDL m2 v2
where
v2 = sparseMulT v m
m2 = decompLDL $ lsqMatrix m
lsqSolveDist :: (Fractional a, Unbox a) =>
SparseMatrix (a, a)
-> V.Vector (a, a)
-> V.Vector a
lsqSolveDist (SparseMatrix r s m') v
| rows m' /= V.length v = error "lsqSolve: lengths don't match"
| otherwise = solveLDL m3 v3
where
v3 = sparseMulT v1 m1 `addVec` sparseMulT v2 m2
m3 = decompLDL $ lsqMatrix m1 `addMatrix` lsqMatrix m2
(v1, v2) = V.unzip v
(m1', m2') = M.unzip m'
m1 = SparseMatrix r s m1'
m2 = SparseMatrix r s m2'