module Data.Vec.LinAlg
(dot
,normSq
,norm
,normalize
,cross
,homPoint
,homVec
,project
,multvm
,multmv
,multmm
,translate
,column
,row
,Transpose(transpose)
,SetDiagonal(setDiagonal)
,GetDiagonal(getDiagonal)
,scale
,diagonal
,identity
,det
,cramer'sRule
,NearZero(nearZero)
,GaussElim(gaussElim)
,BackSubstitute(backSubstitute)
,BackSubstitute'(backSubstitute')
,invert
,invertAndDet
,solve
) where
import Prelude hiding (map,zipWith,foldl,foldr,reverse,take,drop,
head,tail,sum,length,last)
import qualified Prelude as P
import Data.Vec.Base
import Data.Vec.Nat
import Data.Vec.Instances
import Control.Monad
import Data.Maybe
dot :: (Num a, Num v, Fold a v) => v -> v -> a
dot u v = sum (u*v)
normSq :: (Num a, Num v, Fold a v) => v -> a
normSq v = dot v v
norm :: (Num v, Floating a, Fold a v) => v -> a
norm v = sqrt (dot v v)
normalize :: (Floating a, Num v, Fold a v, Map a a v v) => v -> v
normalize v = map (/(norm v)) v
cross :: Num a => Vec3 a -> Vec3 a -> Vec3 a
cross (ux:.uy:.uz:.()) (vx:.vy:.vz:.()) =
(uy*vzuz*vy):.(uz*vxux*vz):.(ux*vyuy*vx):.()
homPoint :: (Snoc v a v', Num a) => v -> v'
homPoint v = snoc v 1
homVec :: (Snoc v a v', Num a) => v -> v'
homVec v = snoc v 0
project ::
( Reverse' () t1 v'
, Fractional t1
, Vec a t t1
, Reverse' () v (t :. t1)
) => v -> v'
project v = case reverse v of (w:.u) -> reverse (u/vec w)
multvm ::
( Transpose m mt
, Map v a mt v'
, Fold a v
, Num a
, Num v
) => v -> m -> v'
multvm v m = map (dot v) (transpose m)
multmv ::
( Map v a m v'
, Num v
, Fold a v
, Num a
) => m -> v -> v'
multmv m v = map (dot v) m
multmm ::
(Map v v' m1 m3
,Map v a b v'
,Transpose m2 b
,Fold a v
,Num v
,Num a
) => m1 -> m2 -> m3
multmm a b = map (\v -> map (dot v) (transpose b)) a
translate ::
(Transpose m mt
,Reverse' () mt (v' :. t)
,Reverse' (v' :. ()) t v'1
,Transpose v'1 m
,Num v'
,Num a
,Snoc v a v'
) => v -> m -> m
translate v m =
case reverse (transpose m) of
(h:.t) -> transpose (reverse (((homVec v) + h) :. t))
column :: (Transpose m mt, Access n v mt) => n -> m -> v
column n = get n . transpose
row :: (Access n a v) => n -> v -> a
row n = get n
class Transpose a b | a -> b, b -> a where
transpose :: a -> b
instance Transpose () () where
transpose = id
instance
(Vec (Succ n) s (s:.ra)
,Vec (Succ m) (s:.ra) ((s:.ra):.a)
,Vec (Succ m) s (s:.rb)
,Vec (Succ n) (s:.rb) ((s:.rb):.b)
,Transpose' ((s:.ra):.a) ((s:.rb):.b)
)
=> Transpose ((s:.ra):.a) ((s:.rb):.b)
where
transpose = transpose'
class Transpose' a b | a->b
where transpose' :: a -> b
instance Transpose' () () where
transpose' = id
instance
(Transpose' vs vs') => Transpose' ( () :. vs ) vs'
where
transpose' (():.vs) = transpose' vs
instance Transpose' ((x:.()):.()) ((x:.()):.()) where
transpose' = id
instance
(Head xss_h xss_hh
,Map xss_h xss_hh (xss_h:.xss_t) xs'
,Tail xss_h xss_ht
,Map xss_h xss_ht (xss_h:.xss_t) xss_
,Transpose' (xs :. xss_) xss'
)
=> Transpose' ((x:.xs):.(xss_h:.xss_t)) ((x:.xs'):.xss')
where
transpose' ((x:.xs):.xss) =
(x :. (map head xss)) :. (transpose' (xs :. (map tail xss) :: (xs:.xss_)))
class SetDiagonal v m | m -> v, v -> m where
setDiagonal :: v -> m -> m
instance (Vec n a v, Vec n r m, SetDiagonal' N0 v m) => SetDiagonal v m where
setDiagonal v m = setDiagonal' (undefined::N0) v m
class SetDiagonal' n v m where
setDiagonal' :: n -> v -> m -> m
instance SetDiagonal' n () m where
setDiagonal' _ _ m = m
instance
( SetDiagonal' (Succ n) v m
, Access n a r
) => SetDiagonal' n (a:.v) (r:.m)
where
setDiagonal' _ (a:.v) (r:.m) =
(set (undefined::n) a r) :. (setDiagonal' (undefined::Succ n) v m)
class GetDiagonal m v | m -> v, v -> m where
getDiagonal :: m -> v
instance (Vec n a v, Vec n v m, GetDiagonal' N0 () m v) => GetDiagonal m v where
getDiagonal m = getDiagonal' (undefined::N0) () m
class GetDiagonal' n p m v where
getDiagonal' :: n -> p -> m -> v
instance
(Access n a r
,Append p (a:.()) (a:.p)
) => GetDiagonal' n p (r:.()) (a:.p)
where
getDiagonal' _ p (r:.()) = append p ((get (undefined::n) r) :. ())
instance
(Access n a r
,Append p (a:.()) p'
,GetDiagonal' (Succ n) p' (r:.m) v
)
=> GetDiagonal' n p (r:.r:.m) v
where
getDiagonal' _ p (r:.m) =
getDiagonal' (undefined::Succ n) (append p ((get (undefined::n) r):.())) m
scale ::
( GetDiagonal' N0 () m r
, Num r
, Vec n a r
, Vec n r m
, SetDiagonal' N0 r m
) => r -> m -> m
scale s m = setDiagonal (s * (getDiagonal m)) m
diagonal :: (Vec n a v, Vec n v m, SetDiagonal v m, Num m) => v -> m
diagonal v = setDiagonal v 0
identity :: (Vec n a v, Vec n v m, Num v, Num m, SetDiagonal v m) => m
identity = diagonal 1
class DropConsec v vv | v -> vv where
dropConsec :: v -> vv
instance
(Vec n a v
,Pred n n_
,Vec n_ a v_
,Vec n v_ vv
,DropConsec' () v vv
) => DropConsec v vv
where
dropConsec v = dropConsec' () v
class DropConsec' p v vv where
dropConsec' :: p -> v -> vv
instance DropConsec' p (a:.()) (p:.()) where
dropConsec' p (a:.()) = (p:.())
instance
(Append p (a:.v) x
,Append p (a:.()) y
,DropConsec' y (a:.v) z
)
=> DropConsec' p (a:.a:.v) (x:.z)
where
dropConsec' p (a:.v) =
(append p v) :. (dropConsec' (append p (a:.())) v)
class Alternating n a v | v -> n a where
alternating :: n -> a -> v
instance Alternating N1 a (a:.()) where
alternating _ !a = a:.()
instance (Num a, Alternating n a (a:.v)) => Alternating (Succ n) a (a:.a:.v) where
alternating _ !a = a:.(alternating (undefined::n) (negate $! a))
class Det' a m | m -> a where
det' :: m -> a
instance Num a => Det' a ((a:.a:.()):.(a:.a:.()):.()) where
det' ( (a:.b:.()) :. (c:.d:.()) :. () ) = a*db*c
instance
(Num a
,Num (a:.a:.a:.v)
,Fold a (a:.a:.a:.v)
,Alternating (Succ (Succ (Succ n))) a (a:.a:.a:.v)
,DropConsec (a:.a:.a:.v) vv
,Map (a:.a:.a:.v) vv ((a:.a:.a:.v):.(a:.a:.a:.v):.m) vmt
,Transpose vmt vm
,Map ((a:.a:.v):.(a:.a:.v):.m_) a vm (a:.a:.a:.v)
,Det' a ((a:.a:.v):.(a:.a:.v):.m_)
,Vec (Succ (Succ (Succ n))) a (a:.a:.a:.v)
,Vec (Succ (Succ (Succ n))) (a:.a:.a:.v) ((a:.a:.a:.v):.(a:.a:.a:.v):.(a:.a:.a:.v):.m)
)
=>
Det' a ((a:.a:.a:.v):.(a:.a:.a:.v):.(a:.a:.a:.v):.m)
where
det' (mh:.mt) =
sum ((alternating undefined 1) * mh *
(map det' (transpose (map dropConsec mt :: vmt))))
det :: forall n a r m. (Vec n a r, Vec n r m, Det' a m) => m -> a
det = det'
class ReplConsec a v vv | v->a, v->vv, vv->v, vv->a where
replConsec :: a -> v -> vv
instance
(Vec n a v
,Vec n v vv
,ReplConsec' a () v vv
) => ReplConsec a v vv
where
replConsec a v = replConsec' a () v :: vv
class ReplConsec' a p v vv where
replConsec' :: a -> p -> v -> vv
instance ReplConsec' a p () () where
replConsec' _ _ () = ()
instance
(Append p (a:.v) x
,Append p (a:.()) y
,ReplConsec' a y v z
)
=> ReplConsec' a p (a:.v) (x:.z)
where
replConsec' r p (a:.v) =
(append p (r:.v)) :. (replConsec' r (append p (a :. ())) v)
cramer'sRule ::
(Map a a1 b1 v
,Transpose w b1
,ZipWith a2 b vv v m w
,ReplConsec' a2 () b vv
,Vec n b vv
,Vec n a2 b
,Fractional a1
,Det' a1 m
,Det' a1 a
) => m -> v -> v
cramer'sRule m b =
case map (\m' -> (det' m')/(det' m))
(transpose (zipWith replConsec b m))
of b' -> b' `asTypeOf` b
mapFst f (a,b) = (f a,b)
class Num a => NearZero a where
nearZero :: a -> Bool
nearZero 0 = True
nearZero _ = False
instance NearZero Float where
nearZero x = abs x < 1e-6
instance NearZero Double where
nearZero x = abs x < 1e-14
instance NearZero Rational
class Pivot1 a m where
pivot1 :: m -> Maybe (m,a)
instance Pivot1 a () where
pivot1 _ = Nothing
instance
( Fractional a, NearZero a
) => Pivot1 a ((a:.()):.())
where
pivot1 ((p:._):._)
| nearZero p = Nothing
| otherwise = Just (1,p)
instance
( Fractional a, NearZero a
, Map a a (a:.r) (a:.r)
) => Pivot1 a ((a:.(a:.r)):.())
where
pivot1 ((p:.r):._)
| nearZero p = Nothing
| otherwise = Just ((1 :. (map (/p) r)):.(), p)
instance
( Fractional a, NearZero a
, Map a a (a:.r) (a:.r)
, ZipWith a a a (a:.r) (a:.r) (a:.r)
, Map (a:.r) (a:.r) ((a:.r):.rs) ((a:.r):.rs)
, Pivot1 a ((a:.r):.rs)
) => Pivot1 a ((a:.r):.(a:.r):.rs)
where
pivot1 (row@(p:._):.rows)
| nearZero p = pivot1 rows >>= \(r:.rs,p)-> Just(r:.row:.rs,p)
| otherwise = Just ( first:.(map add rows) , p)
where first = map (/p) row
add r@(x:._) = zipWith () r . map (*x) $ first
class Pivot a m | m -> a where
pivot :: m -> Maybe (m,a)
instance Pivot a (():.v) where
pivot _ = Nothing
instance
( Fractional a
, NearZero a
, Pivot1 a rs
, Tail (a:.r) r
, Map (a:.r) r ((a:.r):.rs) (r:.rs')
, Map r (a:.r) (r:.rs') ((a:.r):.rs)
, Pivot1 a ((a:.r):.rs)
, Pivot a (r:.rs')
) => Pivot a ((a:.r):.rs)
where
pivot m =
mplus (pivot1 m)
(pivot (map tail m) >>= return . mapFst (map (0:.)) )
class GaussElim a m | m -> a where
gaussElim :: m -> (m,a)
instance (Num a, Pivot a (r:.())) => GaussElim a (r:.())
where
gaussElim m = fromMaybe (m,1) (pivot m)
instance
( Fractional a
, Map (a:.r) r ((a:.r):.rs) rs_
, Map r (a:.r) rs_ ((a:.r):.rs)
, Pivot a ((a:.r):.(a:.r):.rs)
, GaussElim a rs_
) => GaussElim a ((a:.r):.(a:.r):.rs)
where
gaussElim m =
flip (maybe (m,1)) (pivot m) $ \(row:.rows,p) ->
case gaussElim (map tail rows)
of (rows',p') -> ( row:.(map (0:.) rows') , p*p')
class BackSubstitute m where
backSubstitute :: m -> Maybe m
instance BackSubstitute ((a:.r):.()) where
backSubstitute = Just . id
instance
( Map (a:.r) r ((a:.r):.rs) rs_
, Map r (a:.r) rs_ ((a:.r):.rs)
, Fold (a,a:.r) aas
, ZipWith a a a (a:.r) (a:.r) (a:.r)
, Map a a (a:.r) (a:.r)
, ZipWith a (a:.r) (a,a:.r) r ((a:.r):.rs) aas
, Num a, NearZero a
, BackSubstitute rs_
) => BackSubstitute ((a:.r):.(a:.r):.rs)
where
backSubstitute (r@(rh:.rt):.rs)
| nearZero (1rh) =
liftM (map (0:.)) (backSubstitute . map tail $ rs) >>= \rs' ->
return . (:.rs') . foldl (\v (a,w) -> sub v a w) r $
zipWith (,) rt rs'
| otherwise = Nothing
where sub v a = zipWith () v . map (*a)
class BackSubstitute' m where
backSubstitute' :: m -> m
instance BackSubstitute' ((a:.r):.()) where
backSubstitute' = id
instance
( Map (a:.r) r ((a:.r):.rs) rs_
, Map r (a:.r) rs_ ((a:.r):.rs)
, Fold (a,a:.r) aas
, ZipWith a a a (a:.r) (a:.r) (a:.r)
, Map a a (a:.r) (a:.r)
, ZipWith a (a:.r) (a,a:.r) r ((a:.r):.rs) aas
, Num a
, BackSubstitute' rs_
) => BackSubstitute' ((a:.r):.(a:.r):.rs)
where
backSubstitute' (r@(_:.rt):.rs) =
case map (0:.) (backSubstitute' . map tail $ rs)
of rs' -> (:.rs') $ foldl (\ v (a,w) -> sub v a w) r
(zipWith (,) rt rs')
where sub v a = zipWith () v . map (*a)
invert :: forall n a r m r' m'.
( Num r, Num m
, Vec n a r
, Vec n r m
, Append r r r'
, ZipWith r r r' m m m'
, Drop n r' r
, Map r' r m' m
, SetDiagonal r m
, GaussElim a m'
, BackSubstitute m'
) => m -> Maybe m
invert m =
return i >>= backSubstitute . fst . gaussElim . zipWith append m
>>= return . map dropn
where dropn = drop (undefined::n)
i = identity :: m
invertAndDet :: forall n a r m r' m'.
( Num r, Num m
, Vec n a r
, Vec n r m
, Append r r r'
, ZipWith r r r' m m m'
, Drop n r' r
, Map r' r m' m
, SetDiagonal r m
, GaussElim a m'
, BackSubstitute' m'
) => m -> (m,a)
invertAndDet m =
mapFst ( (map dropn) . backSubstitute') . gaussElim . zipWith append m $ i
where dropn = drop (undefined::n)
i = identity :: m
solve :: forall n a v r m r' m'.
( Num r, Num m
, Vec n a r
, Vec n r m
, Snoc r a r'
, ZipWith r a r' m r m'
, Drop n r' (a:.())
, Map r' a m' r
, GaussElim a m'
, BackSubstitute m'
) => m -> r -> Maybe r
solve m v =
return v >>= backSubstitute . fst . gaussElim . zipWith snoc m
>>= return . map (head . drop (undefined::n))