{-# LANGUAGE BangPatterns #-}
module Geom2D.CubicBezier.Numeric
(quadraticRoot, cubicRoot, cubicRootNorm, solveLinear2x2,
goldSearch,
makeSparse, SparseMatrix, sparseMulT, sparseMul,
addMatrix, addVec, lsqMatrix, lsqSolveDist,
decompLDL, lsqSolve,
solveTriDiagonal, solveCyclicTriD)
where
import qualified Data.Vector.Unboxed as V
import Data.Vector.Unboxed.Mutable as MV
import Data.Matrix.Unboxed as M
import qualified Data.Matrix.Class as G
import qualified Data.Matrix.Unboxed.Mutable as MM
import Control.Monad.ST
import Control.Monad
sign :: (Ord a, Num a, Num t) => a -> t
sign :: forall a t. (Ord a, Num a, Num t) => a -> t
sign a
x | a
x forall a. Ord a => a -> a -> Bool
< a
0 = -t
1
| Bool
otherwise = t
1
quadraticRoot :: Double -> Double -> Double -> [Double]
quadraticRoot :: Double -> Double -> Double -> [Double]
quadraticRoot Double
a Double
b Double
c
| Double
a forall a. Eq a => a -> a -> Bool
== Double
0 Bool -> Bool -> Bool
&& Double
b forall a. Eq a => a -> a -> Bool
== Double
0 = []
| Double
a forall a. Eq a => a -> a -> Bool
== Double
0 = [-Double
cforall a. Fractional a => a -> a -> a
/Double
b]
| Bool
otherwise = [Double]
result
where
d :: Double
d = Double
bforall a. Num a => a -> a -> a
*Double
b forall a. Num a => a -> a -> a
- Double
4forall a. Num a => a -> a -> a
*Double
aforall a. Num a => a -> a -> a
*Double
c
q :: Double
q = - (Double
b forall a. Num a => a -> a -> a
+ forall a t. (Ord a, Num a, Num t) => a -> t
sign Double
b forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
sqrt Double
d) forall a. Fractional a => a -> a -> a
/ Double
2
x1 :: Double
x1 = Double
qforall a. Fractional a => a -> a -> a
/Double
a
x2 :: Double
x2 = Double
cforall a. Fractional a => a -> a -> a
/Double
q
result :: [Double]
result | Double
d forall a. Ord a => a -> a -> Bool
< Double
0 = []
| Double
d forall a. Eq a => a -> a -> Bool
== Double
0 = [Double
x1]
| Bool
otherwise = [Double
x1, Double
x2]
cubicRoot :: Double -> Double -> Double -> Double -> [Double]
cubicRoot :: Double -> Double -> Double -> Double -> [Double]
cubicRoot Double
a Double
b Double
c Double
d
| Double
a forall a. Eq a => a -> a -> Bool
== Double
0 = Double -> Double -> Double -> [Double]
quadraticRoot Double
b Double
c Double
d
| Bool
otherwise = Double -> Double -> Double -> [Double]
cubicRootNorm (Double
bforall a. Fractional a => a -> a -> a
/Double
a) (Double
cforall a. Fractional a => a -> a -> a
/Double
a) (Double
dforall a. Fractional a => a -> a -> a
/Double
a)
cubicRootNorm :: Double -> Double -> Double -> [Double]
cubicRootNorm :: Double -> Double -> Double -> [Double]
cubicRootNorm Double
a Double
b Double
c
| Double
r2 forall a. Ord a => a -> a -> Bool
< Double
q3 = [Double
m2sqrtQ forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
cos(Double
tforall a. Fractional a => a -> a -> a
/Double
3) forall a. Num a => a -> a -> a
- Double
aforall a. Fractional a => a -> a -> a
/Double
3,
Double
m2sqrtQ forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
cos((Double
tforall a. Num a => a -> a -> a
+Double
2forall a. Num a => a -> a -> a
*forall a. Floating a => a
pi)forall a. Fractional a => a -> a -> a
/Double
3) forall a. Num a => a -> a -> a
- Double
aforall a. Fractional a => a -> a -> a
/Double
3,
Double
m2sqrtQ forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
cos((Double
t forall a. Num a => a -> a -> a
- Double
2forall a. Num a => a -> a -> a
*forall a. Floating a => a
pi)forall a. Fractional a => a -> a -> a
/Double
3) forall a. Num a => a -> a -> a
- Double
aforall a. Fractional a => a -> a -> a
/Double
3]
| Bool
otherwise = [Double
dforall a. Num a => a -> a -> a
+Double
eforall a. Num a => a -> a -> a
-Double
aforall a. Fractional a => a -> a -> a
/Double
3]
where q :: Double
q = (Double
aforall a. Num a => a -> a -> a
*Double
a forall a. Num a => a -> a -> a
- Double
3forall a. Num a => a -> a -> a
*Double
b) forall a. Fractional a => a -> a -> a
/ Double
9
q3 :: Double
q3 = Double
qforall a. Num a => a -> a -> a
*Double
qforall a. Num a => a -> a -> a
*Double
q
r :: Double
r = (Double
2forall a. Num a => a -> a -> a
*Double
aforall a. Num a => a -> a -> a
*Double
aforall a. Num a => a -> a -> a
*Double
a forall a. Num a => a -> a -> a
- Double
9forall a. Num a => a -> a -> a
*Double
aforall a. Num a => a -> a -> a
*Double
b forall a. Num a => a -> a -> a
+ Double
27forall a. Num a => a -> a -> a
*Double
c) forall a. Fractional a => a -> a -> a
/ Double
54
t :: Double
t = forall a. Floating a => a -> a
acos(Double
rforall a. Fractional a => a -> a -> a
/forall a. Floating a => a -> a
sqrt Double
q3)
m2sqrtQ :: Double
m2sqrtQ = -Double
2forall a. Num a => a -> a -> a
*forall a. Floating a => a -> a
sqrt Double
q
r2 :: Double
r2 = Double
rforall a. Num a => a -> a -> a
*Double
r
d :: Double
d = - forall a t. (Ord a, Num a, Num t) => a -> t
sign(Double
r)forall a. Num a => a -> a -> a
*((forall a. Num a => a -> a
abs Double
r forall a. Num a => a -> a -> a
+ forall a. Floating a => a -> a
sqrt(Double
r2forall a. Num a => a -> a -> a
-Double
q3))forall a. Floating a => a -> a -> a
**Double
1forall a. Fractional a => a -> a -> a
/Double
3)
e :: Double
e | Double
d forall a. Eq a => a -> a -> Bool
== Double
0 = Double
0
| Bool
otherwise = Double
qforall a. Fractional a => a -> a -> a
/Double
d
solveLinear2x2 :: (Eq a, Fractional a) => a -> a -> a -> a -> a -> a -> Maybe (a, a)
solveLinear2x2 :: forall a.
(Eq a, Fractional a) =>
a -> a -> a -> a -> a -> a -> Maybe (a, a)
solveLinear2x2 a
a a
b a
c a
d a
e a
f =
case a
det of a
0 -> forall a. Maybe a
Nothing
a
_ -> forall a. a -> Maybe a
Just ((a
c forall a. Num a => a -> a -> a
* a
e forall a. Num a => a -> a -> a
- a
b forall a. Num a => a -> a -> a
* a
f) forall a. Fractional a => a -> a -> a
/ a
det, (a
a forall a. Num a => a -> a -> a
* a
f forall a. Num a => a -> a -> a
- a
c forall a. Num a => a -> a -> a
* a
d) forall a. Fractional a => a -> a -> a
/ a
det)
where det :: a
det = a
d forall a. Num a => a -> a -> a
* a
b forall a. Num a => a -> a -> a
- a
a forall a. Num a => a -> a -> a
* a
e
{-# SPECIALIZE solveLinear2x2 :: Double -> Double -> Double -> Double -> Double -> Double -> Maybe (Double, Double) #-}
phi :: (Floating a) => a
phi :: forall a. Floating a => a
phi = (-a
1 forall a. Num a => a -> a -> a
+ forall a. Floating a => a -> a
sqrt a
5) forall a. Fractional a => a -> a -> a
/ a
2
goldSearch :: (Ord a, Floating a) => (a -> a) -> Int -> a
goldSearch :: forall a. (Ord a, Floating a) => (a -> a) -> Int -> a
goldSearch a -> a
f Int
maxiter =
forall a.
(Ord a, Floating a) =>
(a -> a) -> a -> a -> a -> a -> a -> a -> a -> a -> Int -> a
goldSearch' a -> a
f a
0 a
x1 a
x2 a
1 (a -> a
f a
0)
(a -> a
f a
x1) (a -> a
f a
x2) (a -> a
f a
1) Int
maxiter
where x1 :: a
x1 = a
1 forall a. Num a => a -> a -> a
- forall a. Floating a => a
phi
x2 :: a
x2 = forall a. Floating a => a
phi
{-# SPECIALIZE goldSearch :: (Double -> Double) -> Int -> Double #-}
goldSearch' :: (Ord a, Floating a) =>
(a -> a) -> a -> a -> a ->
a -> a -> a -> a -> a -> Int -> a
goldSearch' :: forall a.
(Ord a, Floating a) =>
(a -> a) -> a -> a -> a -> a -> a -> a -> a -> a -> Int -> a
goldSearch' a -> a
f a
x0 a
x1 a
x2 a
x3 a
y0 a
y1 a
y2 a
y3 Int
maxiter
| Int
maxiter forall a. Ord a => a -> a -> Bool
< Int
1 = forall a b. (a, b) -> b
snd forall a b. (a -> b) -> a -> b
$ forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [(a
y0, a
x0), (a
y1, a
x1),
(a
y2, a
x2), (a
y3, a
x3)]
| a
y1 forall a. Ord a => a -> a -> Bool
< a
y2 =
let x25 :: a
x25 = a
x1 forall a. Num a => a -> a -> a
+ forall a. Floating a => a
phiforall a. Num a => a -> a -> a
*(a
x3forall a. Num a => a -> a -> a
-a
x1)
y25 :: a
y25 = a -> a
f a
x25
in forall a.
(Ord a, Floating a) =>
(a -> a) -> a -> a -> a -> a -> a -> a -> a -> a -> Int -> a
goldSearch' a -> a
f a
x1 a
x2 a
x25 a
x3 a
y1 a
y2 a
y25 a
y3 (Int
maxiterforall a. Num a => a -> a -> a
-Int
1)
| Bool
otherwise =
let x05 :: a
x05 = a
x2 forall a. Num a => a -> a -> a
+ forall a. Floating a => a
phiforall a. Num a => a -> a -> a
*(a
x0forall a. Num a => a -> a -> a
-a
x2)
y05 :: a
y05 = a -> a
f a
x05
in forall a.
(Ord a, Floating a) =>
(a -> a) -> a -> a -> a -> a -> a -> a -> a -> a -> Int -> a
goldSearch' a -> a
f a
x0 a
x05 a
x1 a
x2 a
y0 a
y05 a
y1 a
y2 (Int
maxiterforall a. Num a => a -> a -> a
-Int
1)
data SparseMatrix a =
SparseMatrix (V.Vector Int)
(V.Vector (Int, Int)) (M.Matrix a)
makeSparse :: Unbox a => V.Vector Int
-> M.Matrix a
-> SparseMatrix a
makeSparse :: forall a. Unbox a => Vector Int -> Matrix a -> SparseMatrix a
makeSparse Vector Int
v Matrix a
m = forall a.
Vector Int -> Vector (Int, Int) -> Matrix a -> SparseMatrix a
SparseMatrix Vector Int
v (Vector Int -> Int -> Int -> Vector (Int, Int)
sparseRanges Vector Int
v Int
vars Int
width) Matrix a
m
where
width :: Int
width = forall a. Context a => Matrix a -> Int
cols Matrix a
m
vars :: Int
vars = forall a. Unbox a => Vector a -> a
V.last Vector Int
v forall a. Num a => a -> a -> a
+ Int
width
sparseRanges :: V.Vector Int -> Int -> Int -> V.Vector (Int, Int)
sparseRanges :: Vector Int -> Int -> Int -> Vector (Int, Int)
sparseRanges Vector Int
v Int
vars Int
width = Vector (Int, Int)
ranges
where
height :: Int
height = forall a. Unbox a => Vector a -> Int
V.length Vector Int
v
ranges :: Vector (Int, Int)
ranges = forall a b.
(Unbox a, Unbox b) =>
(a -> b -> a) -> a -> Vector b -> Vector a
V.scanl' (Int, Int) -> Int -> (Int, Int)
nextRange ((Int, Int) -> Int -> (Int, Int)
nextRange (Int
0,Int
0) Int
0) forall a b. (a -> b) -> a -> b
$
forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
1 (Int
varsforall a. Num a => a -> a -> a
-Int
1)
nextRange :: (Int, Int) -> Int -> (Int, Int)
nextRange (Int
s,Int
e) Int
i = (Int -> Int -> Int
nextStart Int
s Int
i, Int -> Int -> Int
nextEnd Int
e Int
i)
nextStart :: Int -> Int -> Int
nextStart Int
s Int
i
| Int
s forall a. Ord a => a -> a -> Bool
>= Int
height = Int
height
| Vector Int
v forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
s forall a. Num a => a -> a -> a
+ Int
width forall a. Ord a => a -> a -> Bool
<= Int
i =
Int -> Int -> Int
nextStart (Int
sforall a. Num a => a -> a -> a
+Int
1) Int
i
| Bool
otherwise = Int
s
nextEnd :: Int -> Int -> Int
nextEnd Int
e Int
i
| Int
e forall a. Ord a => a -> a -> Bool
>= Int
height = Int
height
| Vector Int
v forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
e forall a. Ord a => a -> a -> Bool
> Int
i = Int
e
| Bool
otherwise = Int -> Int -> Int
nextEnd (Int
eforall a. Num a => a -> a -> a
+Int
1) Int
i
lsqMatrix :: (Num a, Unbox a) =>
SparseMatrix a
-> Matrix a
lsqMatrix :: forall a. (Num a, Unbox a) => SparseMatrix a -> Matrix a
lsqMatrix (SparseMatrix Vector Int
rowStart Vector (Int, Int)
ranges Matrix a
m)
| forall a. Unbox a => Vector a -> Int
V.length Vector Int
rowStart forall a. Eq a => a -> a -> Bool
/= Int
height =
forall a. HasCallStack => [Char] -> a
error [Char]
"lsqMatrix: lengths don't match."
| Bool
otherwise = forall a. Context a => (Int, Int) -> ((Int, Int) -> a) -> Matrix a
M.generate (Int
vars, Int
width) (Int, Int) -> a
coeff
where
(Int
height, Int
width) = forall a. Context a => Matrix a -> (Int, Int)
dim Matrix a
m
vars :: Int
vars = forall a. Unbox a => Vector a -> a
V.last Vector Int
rowStart forall a. Num a => a -> a -> a
+ Int
width
overlap :: (a, b) -> (a, b) -> (a, b)
overlap (a
s1,b
e1) (a
s2, b
e2) =
(forall a. Ord a => a -> a -> a
max a
s1 a
s2, forall a. Ord a => a -> a -> a
min b
e1 b
e2)
realIndex :: (Int, Int) -> (Int, Int)
realIndex (Int
r, Int
c) =
(Int
r, Int
c forall a. Num a => a -> a -> a
- Vector Int
rowStart forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
r)
coeff :: (Int, Int) -> a
coeff (Int
r,Int
c) = let
(Int
s, Int
e) | Int
rforall a. Num a => a -> a -> a
+Int
c forall a. Ord a => a -> a -> Bool
>= Int
vars = (Int
0, Int
0)
| Bool
otherwise =
forall {a} {b}. (Ord a, Ord b) => (a, b) -> (a, b) -> (a, b)
overlap (Vector (Int, Int)
ranges forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
r) (Vector (Int, Int)
ranges forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` (Int
rforall a. Num a => a -> a -> a
+Int
c))
in forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (\a
acc Int
i -> a
acc forall a. Num a => a -> a -> a
+ Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int, Int) -> (Int, Int)
realIndex (Int
i, Int
r) forall a. Num a => a -> a -> a
*
Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int, Int) -> (Int, Int)
realIndex (Int
i, Int
rforall a. Num a => a -> a -> a
+Int
c)) a
0 forall a b. (a -> b) -> a -> b
$
forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
s (Int
eforall a. Num a => a -> a -> a
-Int
s)
{-# SPECIALIZE lsqMatrix :: SparseMatrix Double -> M.Matrix Double #-}
addMatrix :: (Num a, Unbox a) => M.Matrix a -> M.Matrix a -> M.Matrix a
addMatrix :: forall a. (Num a, Unbox a) => Matrix a -> Matrix a -> Matrix a
addMatrix = forall a b c.
(Context a, Context b, Context c) =>
(a -> b -> c) -> Matrix a -> Matrix b -> Matrix c
M.zipWith forall a. Num a => a -> a -> a
(+)
{-# SPECIALIZE addMatrix :: M.Matrix Double -> M.Matrix Double -> M.Matrix Double #-}
addVec :: (Num a, Unbox a) => V.Vector a -> V.Vector a -> V.Vector a
addVec :: forall a. (Num a, Unbox a) => Vector a -> Vector a -> Vector a
addVec = forall a b c.
(Unbox a, Unbox b, Unbox c) =>
(a -> b -> c) -> Vector a -> Vector b -> Vector c
V.zipWith forall a. Num a => a -> a -> a
(+)
{-# SPECIALIZE addVec :: V.Vector Double -> V.Vector Double -> V.Vector Double #-}
sparseMulT :: (Num a, Unbox a) =>
V.Vector a
-> SparseMatrix a
-> V.Vector a
sparseMulT :: forall a.
(Num a, Unbox a) =>
Vector a -> SparseMatrix a -> Vector a
sparseMulT Vector a
v (SparseMatrix Vector Int
rowStart Vector (Int, Int)
ranges Matrix a
m)
| forall a. Unbox a => Vector a -> Int
V.length Vector a
v forall a. Eq a => a -> a -> Bool
/= Int
height =
forall a. HasCallStack => [Char] -> a
error [Char]
"sparseMulT: lengths don't match."
| Bool
otherwise = forall a. Unbox a => Int -> (Int -> a) -> Vector a
V.generate Int
vars Int -> a
coeff
where (Int
height, Int
width) = forall a. Context a => Matrix a -> (Int, Int)
dim Matrix a
m
vars :: Int
vars | forall a. Unbox a => Vector a -> Bool
V.null Vector Int
rowStart = Int
0
| Bool
otherwise = forall a. Unbox a => Vector a -> a
V.unsafeLast Vector Int
rowStart forall a. Num a => a -> a -> a
+ Int
width
realIndex :: (Int, Int) -> (Int, Int)
realIndex (Int
r, Int
c) =
(Int
r, Int
c forall a. Num a => a -> a -> a
- Vector Int
rowStart forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
r)
coeff :: Int -> a
coeff Int
i =
let (Int
s, Int
e) = Vector (Int, Int)
ranges forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
i
in forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (\a
acc Int
j ->
a
acc forall a. Num a => a -> a -> a
+ Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int, Int) -> (Int, Int)
realIndex (Int
j, Int
i) forall a. Num a => a -> a -> a
*
Vector a
v forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
j) a
0 forall a b. (a -> b) -> a -> b
$
forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
s (Int
eforall a. Num a => a -> a -> a
-Int
s)
{-# SPECIALIZE sparseMulT :: V.Vector Double -> SparseMatrix Double -> V.Vector Double #-}
sparseMul :: (Num a, Unbox a) =>
SparseMatrix a
-> V.Vector a
-> V.Vector a
sparseMul :: forall a.
(Num a, Unbox a) =>
SparseMatrix a -> Vector a -> Vector a
sparseMul (SparseMatrix Vector Int
rowStart Vector (Int, Int)
_ranges Matrix a
m) Vector a
v
| forall a. Unbox a => Vector a -> Int
V.length Vector a
v forall a. Eq a => a -> a -> Bool
/= Int
vars =
forall a. HasCallStack => [Char] -> a
error [Char]
"sparseMulT: lengths don't match."
| Bool
otherwise = forall a. Unbox a => Int -> (Int -> a) -> Vector a
V.generate Int
height Int -> a
coeff
where (Int
height, Int
width) = forall a. Context a => Matrix a -> (Int, Int)
dim Matrix a
m
vars :: Int
vars | forall a. Unbox a => Vector a -> Bool
V.null Vector Int
rowStart = Int
0
| Bool
otherwise = forall a. Unbox a => Vector a -> a
V.unsafeLast Vector Int
rowStart forall a. Num a => a -> a -> a
+ Int
width
coeff :: Int -> a
coeff Int
i = forall a. (Unbox a, Num a) => Vector a -> a
V.sum forall a b. (a -> b) -> a -> b
$ forall a b c.
(Unbox a, Unbox b, Unbox c) =>
(a -> b -> c) -> Vector a -> Vector b -> Vector c
V.zipWith forall a. Num a => a -> a -> a
(*)
(forall a. Unbox a => Int -> Int -> Vector a -> Vector a
V.unsafeSlice (Vector Int
rowStart forall a. Unbox a => Vector a -> Int -> a
V.! Int
i) Int
width Vector a
v)
(forall (m :: (* -> *) -> * -> *) (v :: * -> *) a.
Matrix m v a =>
m v a -> Int -> v a
G.unsafeTakeRow Matrix a
m Int
i)
{-# SPECIALIZE sparseMul :: SparseMatrix Double -> V.Vector Double -> V.Vector Double #-}
decompLDL :: (Fractional a, Unbox a) => M.Matrix a -> M.Matrix a
decompLDL :: forall a. (Fractional a, Unbox a) => Matrix a -> Matrix a
decompLDL Matrix a
m = forall a. (forall s. ST s a) -> a
runST forall a b. (a -> b) -> a -> b
$ do
MMatrix s a
m2 <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
Matrix a -> s (MMatrix (PrimState s) a)
M.thaw Matrix a
m
let (Int
vars, Int
width) = forall a. Context a => Matrix a -> (Int, Int)
dim Matrix a
m
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 forall a b. (a -> b) -> a -> b
$ Int
varsforall a. Num a => a -> a -> a
-Int
1) forall a b. (a -> b) -> a -> b
$
\Int
startr -> do
a
pivot <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> s a
MM.unsafeRead MMatrix s a
m2 (Int
startr, Int
0)
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
1 forall a b. (a -> b) -> a -> b
$ Int
widthforall a. Num a => a -> a -> a
-Int
1) forall a b. (a -> b) -> a -> b
$
\Int
c -> do
a
el <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> s a
MM.unsafeRead MMatrix s a
m2 (Int
startr, Int
c)
forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> a -> s ()
MM.unsafeWrite MMatrix s a
m2 (Int
startr, Int
c) (a
elforall a. Fractional a => a -> a -> a
/a
pivot)
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 forall a b. (a -> b) -> a -> b
$ forall a. Ord a => a -> a -> a
min (Int
widthforall a. Num a => a -> a -> a
-Int
1) forall a b. (a -> b) -> a -> b
$ Int
varsforall a. Num a => a -> a -> a
-Int
startrforall a. Num a => a -> a -> a
-Int
1) forall a b. (a -> b) -> a -> b
$
\Int
r -> do
a
r0 <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> s a
MM.unsafeRead MMatrix s a
m2 (Int
startr, Int
rforall a. Num a => a -> a -> a
+Int
1)
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 (Int
widthforall a. Num a => a -> a -> a
-Int
rforall a. Num a => a -> a -> a
-Int
1)) forall a b. (a -> b) -> a -> b
$
\Int
c -> do a
r1 <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> s a
MM.unsafeRead MMatrix s a
m2 (Int
startr, Int
rforall a. Num a => a -> a -> a
+Int
cforall a. Num a => a -> a -> a
+Int
1)
a
el <- forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> s a
MM.unsafeRead MMatrix s a
m2 (Int
rforall a. Num a => a -> a -> a
+Int
startrforall a. Num a => a -> a -> a
+Int
1, Int
c)
forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> (Int, Int) -> a -> s ()
MM.unsafeWrite MMatrix s a
m2 (Int
rforall a. Num a => a -> a -> a
+Int
startrforall a. Num a => a -> a -> a
+Int
1, Int
c)
(a
el forall a. Num a => a -> a -> a
- a
r0forall a. Num a => a -> a -> a
*a
r1forall a. Num a => a -> a -> a
*a
pivot)
forall a (s :: * -> *).
(Context a, PrimMonad s) =>
MMatrix (PrimState s) a -> s (Matrix a)
M.unsafeFreeze MMatrix s a
m2
{-# SPECIALIZE decompLDL :: Matrix Double -> Matrix Double #-}
solveLDL :: (Fractional a, Unbox a) =>
M.Matrix a -> V.Vector a -> V.Vector a
solveLDL :: forall a.
(Fractional a, Unbox a) =>
Matrix a -> Vector a -> Vector a
solveLDL Matrix a
m Vector a
v
| forall a. Context a => Matrix a -> Int
rows Matrix a
m forall a. Eq a => a -> a -> Bool
/= forall a. Unbox a => Vector a -> Int
V.length Vector a
v = forall a. HasCallStack => [Char] -> a
error [Char]
"solveLDL: lengths don't match"
| Bool
otherwise = forall a. (forall s. ST s a) -> a
runST forall a b. (a -> b) -> a -> b
$ do
let (Int
vars, Int
width) = forall a. Context a => Matrix a -> (Int, Int)
M.dim Matrix a
m
MVector s a
sol1 <- forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
Int -> m (MVector (PrimState m) a)
MV.new Int
vars
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 forall a b. (a -> b) -> a -> b
$ forall a. Ord a => a -> a -> a
min Int
vars Int
width) forall a b. (a -> b) -> a -> b
$
\Int
i -> do
let vi :: a
vi = Vector a
v forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
i
a
s <- forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM (forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (-) a
vi) forall a b. (a -> b) -> a -> b
$
forall (m :: * -> *) a b.
(Monad m, Unbox a, Unbox b) =>
Vector a -> (a -> m b) -> m (Vector b)
V.forM (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 Int
i) forall a b. (a -> b) -> a -> b
$
\Int
j -> forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM ((Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
j, Int
iforall a. Num a => a -> a -> a
-Int
j)) forall a. Num a => a -> a -> a
*)
(forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 Int
j)
forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.unsafeWrite MVector s a
sol1 Int
i a
s
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
width forall a b. (a -> b) -> a -> b
$ Int
vars forall a. Num a => a -> a -> a
- Int
width) forall a b. (a -> b) -> a -> b
$
\Int
i -> do
let vi :: a
vi = Vector a
v forall a. Unbox a => Vector a -> Int -> a
`V.unsafeIndex` Int
i
a
s <- forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM (forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (-) a
vi) forall a b. (a -> b) -> a -> b
$
forall (m :: * -> *) a b.
(Monad m, Unbox a, Unbox b) =>
Vector a -> (a -> m b) -> m (Vector b)
V.forM (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
1 (Int
widthforall a. Num a => a -> a -> a
-Int
1)) forall a b. (a -> b) -> a -> b
$
\Int
j -> forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM ((Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
iforall a. Num a => a -> a -> a
-Int
j, Int
j)) forall a. Num a => a -> a -> a
*)
(forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 forall a b. (a -> b) -> a -> b
$ Int
iforall a. Num a => a -> a -> a
-Int
j)
forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.unsafeWrite MVector s a
sol1 Int
i a
s
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 forall a b. (a -> b) -> a -> b
$ forall a. Ord a => a -> a -> a
min Int
vars Int
width) forall a b. (a -> b) -> a -> b
$
\Int
i -> do
a
solI <- forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1)
let d :: a
d = Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1, Int
0)
a
s <- forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM (forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (-) (a
solIforall a. Fractional a => a -> a -> a
/a
d)) forall a b. (a -> b) -> a -> b
$
forall (m :: * -> *) a b.
(Monad m, Unbox a, Unbox b) =>
Vector a -> (a -> m b) -> m (Vector b)
V.forM (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 Int
i) forall a b. (a -> b) -> a -> b
$
\Int
j -> forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM ((Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1, Int
jforall a. Num a => a -> a -> a
+Int
1)) forall a. Num a => a -> a -> a
*)
(forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 forall a b. (a -> b) -> a -> b
$ Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
+Int
j)
forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.unsafeWrite MVector s a
sol1 (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1) a
s
forall (m :: * -> *) a b.
(Monad m, Unbox a) =>
Vector a -> (a -> m b) -> m ()
V.forM_ (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
width forall a b. (a -> b) -> a -> b
$ Int
vars forall a. Num a => a -> a -> a
- Int
width) forall a b. (a -> b) -> a -> b
$
\Int
i -> do
a
solI <- forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1)
let d :: a
d = Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1, Int
0)
a
s <- forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM (forall b a. Unbox b => (a -> b -> a) -> a -> Vector b -> a
V.foldl' (-) (a
solIforall a. Fractional a => a -> a -> a
/a
d)) forall a b. (a -> b) -> a -> b
$
forall (m :: * -> *) a b.
(Monad m, Unbox a, Unbox b) =>
Vector a -> (a -> m b) -> m (Vector b)
V.forM (forall a. (Unbox a, Num a) => a -> Int -> Vector a
V.enumFromN Int
0 (Int
widthforall a. Num a => a -> a -> a
-Int
1)) forall a b. (a -> b) -> a -> b
$
\Int
j -> forall (m :: * -> *) a1 r. Monad m => (a1 -> r) -> m a1 -> m r
liftM ((Matrix a
m forall a. Context a => Matrix a -> (Int, Int) -> a
`M.unsafeIndex` (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1, Int
jforall a. Num a => a -> a -> a
+Int
1)) forall a. Num a => a -> a -> a
*)
(forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> m a
MV.unsafeRead MVector s a
sol1 forall a b. (a -> b) -> a -> b
$ Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
+Int
j)
forall (m :: * -> *) a.
(PrimMonad m, Unbox a) =>
MVector (PrimState m) a -> Int -> a -> m ()
MV.unsafeWrite MVector s a
sol1 (Int
varsforall a. Num a => a -> a -> a
-Int
iforall a. Num a => a -> a -> a
-Int
1) a
s
forall a (m :: * -> *).
(Unbox a, PrimMonad m) =>
MVector (PrimState m) a -> m (Vector a)
V.unsafeFreeze MVector s a
sol1
{-# SPECIALIZE solveLDL :: M.Matrix Double -> V.Vector Double -> V.Vector Double #-}
lsqSolve :: (Fractional a, Unbox a) =>
SparseMatrix a
-> V.Vector a
-> V.Vector a
lsqSolve :: forall a.
(Fractional a, Unbox a) =>
SparseMatrix a -> Vector a -> Vector a
lsqSolve m :: SparseMatrix a
m@(SparseMatrix Vector Int
_ Vector (Int, Int)
_ Matrix a
m') Vector a
v
| forall a. Context a => Matrix a -> Int
rows Matrix a
m' forall a. Eq a => a -> a -> Bool
/= forall a. Unbox a => Vector a -> Int
V.length Vector a
v = forall a. HasCallStack => [Char] -> a
error [Char]
"lsqSolve: lengths don't match"
| Bool
otherwise = forall a.
(Fractional a, Unbox a) =>
Matrix a -> Vector a -> Vector a
solveLDL Matrix a
m2 Vector a
v2
where
v2 :: Vector a
v2 = forall a.
(Num a, Unbox a) =>
Vector a -> SparseMatrix a -> Vector a
sparseMulT Vector a
v SparseMatrix a
m
m2 :: Matrix a
m2 = forall a. (Fractional a, Unbox a) => Matrix a -> Matrix a
decompLDL forall a b. (a -> b) -> a -> b
$ forall a. (Num a, Unbox a) => SparseMatrix a -> Matrix a
lsqMatrix SparseMatrix a
m
{-# SPECIALIZE lsqSolve :: SparseMatrix Double -> V.Vector Double -> V.Vector Double #-}
lsqSolveDist :: (Fractional a, Unbox a) =>
SparseMatrix (a, a)
-> V.Vector (a, a)
-> V.Vector a
lsqSolveDist :: forall a.
(Fractional a, Unbox a) =>
SparseMatrix (a, a) -> Vector (a, a) -> Vector a
lsqSolveDist (SparseMatrix Vector Int
r Vector (Int, Int)
s Matrix (a, a)
m') Vector (a, a)
v
| forall a. Context a => Matrix a -> Int
rows Matrix (a, a)
m' forall a. Eq a => a -> a -> Bool
/= forall a. Unbox a => Vector a -> Int
V.length Vector (a, a)
v = forall a. HasCallStack => [Char] -> a
error [Char]
"lsqSolve: lengths don't match"
| Bool
otherwise = forall a.
(Fractional a, Unbox a) =>
Matrix a -> Vector a -> Vector a
solveLDL Matrix a
m3 Vector a
v3
where
v3 :: Vector a
v3 = forall a.
(Num a, Unbox a) =>
Vector a -> SparseMatrix a -> Vector a
sparseMulT Vector a
v1 SparseMatrix a
m1 forall a. (Num a, Unbox a) => Vector a -> Vector a -> Vector a
`addVec` forall a.
(Num a, Unbox a) =>
Vector a -> SparseMatrix a -> Vector a
sparseMulT Vector a
v2 SparseMatrix a
m2
m3 :: Matrix a
m3 = forall a. (Fractional a, Unbox a) => Matrix a -> Matrix a
decompLDL forall a b. (a -> b) -> a -> b
$ forall a. (Num a, Unbox a) => SparseMatrix a -> Matrix a
lsqMatrix SparseMatrix a
m1 forall a. (Num a, Unbox a) => Matrix a -> Matrix a -> Matrix a
`addMatrix` forall a. (Num a, Unbox a) => SparseMatrix a -> Matrix a
lsqMatrix SparseMatrix a
m2
(Vector a
v1, Vector a
v2) = forall a b.
(Unbox a, Unbox b) =>
Vector (a, b) -> (Vector a, Vector b)
V.unzip Vector (a, a)
v
(Matrix a
m1', Matrix a
m2') = forall a b.
(Context a, Context b, Context (a, b)) =>
Matrix (a, b) -> (Matrix a, Matrix b)
M.unzip Matrix (a, a)
m'
m1 :: SparseMatrix a
m1 = forall a.
Vector Int -> Vector (Int, Int) -> Matrix a -> SparseMatrix a
SparseMatrix Vector Int
r Vector (Int, Int)
s Matrix a
m1'
m2 :: SparseMatrix a
m2 = forall a.
Vector Int -> Vector (Int, Int) -> Matrix a -> SparseMatrix a
SparseMatrix Vector Int
r Vector (Int, Int)
s Matrix a
m2'
{-# SPECIALIZE lsqSolveDist :: SparseMatrix (Double, Double) -> V.Vector (Double, Double) -> V.Vector Double #-}
solveTriDiagonal :: (Unbox a, Fractional a) =>
(a, a, a) -> V.Vector (a, a, a, a) -> V.Vector a
solveTriDiagonal :: forall a.
(Unbox a, Fractional a) =>
(a, a, a) -> Vector (a, a, a, a) -> Vector a
solveTriDiagonal (!a
b0, !a
c0, !a
d0) Vector (a, a, a, a)
rows = Vector a
solutions
where
twovars :: Vector (a, a)
twovars = forall a b.
(Unbox a, Unbox b) =>
(a -> b -> a) -> a -> Vector b -> Vector a
V.scanl forall {b}. Fractional b => (b, b) -> (b, b, b, b) -> (b, b)
nextrow (a
c0forall a. Fractional a => a -> a -> a
/a
b0, a
d0forall a. Fractional a => a -> a -> a
/a
b0) Vector (a, a, a, a)
rows
solutions :: Vector a
solutions = forall a b.
(Unbox a, Unbox b) =>
(a -> b -> b) -> b -> Vector a -> Vector b
V.scanr forall {a}. Num a => (a, a) -> a -> a
nextsol a
vn (forall a. Unbox a => Vector a -> Vector a
V.unsafeInit Vector (a, a)
twovars)
vn :: a
vn = forall a b. (a, b) -> b
snd forall a b. (a -> b) -> a -> b
$ forall a. Unbox a => Vector a -> a
V.unsafeLast Vector (a, a)
twovars
nextsol :: (a, a) -> a -> a
nextsol (a
u, a
v) a
ti = a
v forall a. Num a => a -> a -> a
- a
uforall a. Num a => a -> a -> a
*a
ti
nextrow :: (b, b) -> (b, b, b, b) -> (b, b)
nextrow (b
u, b
v) (b
ai, b
bi, b
ci, b
di) =
(b
ciforall a. Fractional a => a -> a -> a
/(b
bi forall a. Num a => a -> a -> a
- b
uforall a. Num a => a -> a -> a
*b
ai), (b
di forall a. Num a => a -> a -> a
- b
vforall a. Num a => a -> a -> a
*b
ai)forall a. Fractional a => a -> a -> a
/(b
bi forall a. Num a => a -> a -> a
- b
uforall a. Num a => a -> a -> a
*b
ai))
{-# SPECIALIZE solveTriDiagonal :: (Double, Double, Double) -> V.Vector (Double, Double, Double, Double) -> V.Vector Double #-}
solveCyclicTriD :: (Unbox a, Fractional a) => V.Vector (a, a, a, a) -> V.Vector a
solveCyclicTriD :: forall a.
(Unbox a, Fractional a) =>
Vector (a, a, a, a) -> Vector a
solveCyclicTriD Vector (a, a, a, a)
rows = Vector a
solutions
where
threevars :: Vector (a, a, a)
threevars = forall a. Unbox a => Vector a -> Vector a
V.tail forall a b. (a -> b) -> a -> b
$ forall a b.
(Unbox a, Unbox b) =>
(a -> b -> a) -> a -> Vector b -> Vector a
V.scanl forall {c}. Fractional c => (c, c, c) -> (c, c, c, c) -> (c, c, c)
nextrow (a
0, a
0, a
1) Vector (a, a, a, a)
rows
nextrow :: (c, c, c) -> (c, c, c, c) -> (c, c, c)
nextrow (!c
u, !c
v, !c
w) (!c
ai, !c
bi, !c
ci, !c
di) =
(c
ciforall a. Fractional a => a -> a -> a
/(c
bi forall a. Num a => a -> a -> a
- c
aiforall a. Num a => a -> a -> a
*c
u), (c
di forall a. Num a => a -> a -> a
- c
aiforall a. Num a => a -> a -> a
*c
v)forall a. Fractional a => a -> a -> a
/(c
bi forall a. Num a => a -> a -> a
- c
aiforall a. Num a => a -> a -> a
*c
u), -c
aiforall a. Num a => a -> a -> a
*c
wforall a. Fractional a => a -> a -> a
/(c
bi forall a. Num a => a -> a -> a
- c
aiforall a. Num a => a -> a -> a
*c
u))
(a
totvn, a
totwn) = forall a b. Unbox a => (a -> b -> b) -> b -> Vector a -> b
V.foldr (\(a
u, a
v, a
w) (a
v', a
w') ->
(a
v forall a. Num a => a -> a -> a
- a
uforall a. Num a => a -> a -> a
*a
v', a
w forall a. Num a => a -> a -> a
- a
uforall a. Num a => a -> a -> a
*a
w'))
(a
0, a
1) (forall a. Unbox a => Vector a -> Vector a
V.init Vector (a, a, a)
threevars)
t0 :: a
t0 = (a
vn forall a. Num a => a -> a -> a
- a
unforall a. Num a => a -> a -> a
*a
totvn) forall a. Fractional a => a -> a -> a
/ (a
1 forall a. Num a => a -> a -> a
- (a
wn forall a. Num a => a -> a -> a
- a
unforall a. Num a => a -> a -> a
*a
totwn))
(!a
un, !a
vn, !a
wn) = forall a. Unbox a => Vector a -> a
V.last Vector (a, a, a)
threevars
solutions :: Vector a
solutions = forall a b.
(Unbox a, Unbox b) =>
(a -> b -> a) -> a -> Vector b -> Vector a
V.scanl a -> (a, a, a) -> a
nextsol a
t0 forall a b. (a -> b) -> a -> b
$ forall a. Unbox a => a -> Vector a -> Vector a
V.cons (a
un, a
vn, a
wn) forall a b. (a -> b) -> a -> b
$
forall a. Unbox a => Vector a -> Vector a
V.init forall a b. (a -> b) -> a -> b
$ forall a. Unbox a => Vector a -> Vector a
V.init forall a b. (a -> b) -> a -> b
$ Vector (a, a, a)
threevars
nextsol :: a -> (a, a, a) -> a
nextsol a
t (!a
u, !a
v, !a
w) = (a
v forall a. Num a => a -> a -> a
+ a
wforall a. Num a => a -> a -> a
*a
t0 forall a. Num a => a -> a -> a
- a
t)forall a. Fractional a => a -> a -> a
/a
u
{-# SPECIALIZE solveCyclicTriD :: V.Vector (Double, Double, Double, Double) -> V.Vector Double #-}