factor/extra/math/matrices/simd/simd.factor

! (c)Joe Groff bsd license
USING: accessors classes.struct kernel locals math
math.matrices.simd math.vectors math.vectors.simd sequences
sequences.private specialized-arrays typed ;
QUALIFIED-WITH: alien.c-types c
SIMD: c:float
SPECIALIZED-ARRAY: float-4
IN: math.matrices.simd

STRUCT: matrix4
    { rows float-4[4] } ;

INSTANCE: matrix4 immutable-sequence

M: matrix4 length drop 4 ; inline
M: matrix4 nth-unsafe rows>> nth-unsafe ; inline
M: matrix4 new-sequence 2drop matrix4 (struct) ; inline

<PRIVATE
:: 2map-rows ( a b quot -- c )
    matrix4 (struct) :> c

    a rows>> first  :> a1
    a rows>> second :> a2
    a rows>> third  :> a3
    a rows>> fourth :> a4
    b rows>> first  :> b1
    b rows>> second :> b2
    b rows>> third  :> b3
    b rows>> fourth :> b4

    a1 b1 quot call :> c1
    a2 b2 quot call :> c2
    a3 b3 quot call :> c3
    a4 b4 quot call :> c4

    c1 c rows>> set-first
    c2 c rows>> set-second
    c3 c rows>> set-third
    c4 c rows>> set-fourth

    c ; inline

:: map-rows ( a quot -- c )
    matrix4 (struct) :> c

    a rows>> first  :> a1
    a rows>> second :> a2
    a rows>> third  :> a3
    a rows>> fourth :> a4

    a1 quot call :> c1
    a2 quot call :> c2
    a3 quot call :> c3
    a4 quot call :> c4

    c1 c rows>> set-first
    c2 c rows>> set-second
    c3 c rows>> set-third
    c4 c rows>> set-fourth

    c ; inline
    
PRIVATE>

TYPED: m4+ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v+ ] 2map-rows ;
TYPED: m4- ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v- ] 2map-rows ;
TYPED: m4* ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v* ] 2map-rows ;
TYPED: m4/ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v/ ] 2map-rows ;

TYPED: m4*n ( a: matrix4 b: float -- c: matrix4 ) [ v*n ] curry map-rows ;
TYPED: m4/n ( a: matrix4 b: float -- c: matrix4 ) [ v/n ] curry map-rows ;
TYPED: n*m4 ( a: float b: matrix4 -- c: matrix4 ) [ n*v ] with map-rows ;
TYPED: n/m4 ( a: float b: matrix4 -- c: matrix4 ) [ n/v ] with map-rows ;

TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
    matrix4 (struct) :> c

    a rows>> first  :> a1
    a rows>> second :> a2
    a rows>> third  :> a3
    a rows>> fourth :> a4
    b rows>> first  :> b1
    b rows>> second :> b2
    b rows>> third  :> b3
    b rows>> fourth :> b4

    a1 { 0 0 0 0 } vshuffle b1 v* :> c1a
    a2 { 0 0 0 0 } vshuffle b1 v* :> c2a
    a3 { 0 0 0 0 } vshuffle b1 v* :> c3a
    a4 { 0 0 0 0 } vshuffle b1 v* :> c4a

    a1 { 1 1 1 1 } vshuffle b2 v* c1a v+ :> c1b 
    a2 { 1 1 1 1 } vshuffle b2 v* c2a v+ :> c2b
    a3 { 1 1 1 1 } vshuffle b2 v* c3a v+ :> c3b
    a4 { 1 1 1 1 } vshuffle b2 v* c4a v+ :> c4b

    a1 { 2 2 2 2 } vshuffle b3 v* c1b v+ :> c1c 
    a2 { 2 2 2 2 } vshuffle b3 v* c2b v+ :> c2c
    a3 { 2 2 2 2 } vshuffle b3 v* c3b v+ :> c3c
    a4 { 2 2 2 2 } vshuffle b3 v* c4b v+ :> c4c

    a1 { 3 3 3 3 } vshuffle b4 v* c1c v+ :> c1 
    a2 { 3 3 3 3 } vshuffle b4 v* c2c v+ :> c2
    a3 { 3 3 3 3 } vshuffle b4 v* c3c v+ :> c3
    a4 { 3 3 3 3 } vshuffle b4 v* c4c v+ :> c4

    c1 c rows>> set-first
    c2 c rows>> set-second
    c3 c rows>> set-third
    c4 c rows>> set-fourth

    c ;

CONSTANT: identity-matrix4
    S{ matrix4 f
        float-4-array{
            float-4{ 1.0 0.0 0.0 0.0 }
            float-4{ 0.0 1.0 0.0 0.0 }
            float-4{ 0.0 0.0 1.0 0.0 }
            float-4{ 0.0 0.0 0.0 1.0 }
        }
    }

TYPED:: scale-matrix4 ( factors: float-4 -- matrix: matrix4 )
    matrix4 (struct) :> c

    factors { t t t f } vmask :> factors'
    factors' { 0 3 3 3 } vshuffle :> c1
    factors' { 3 1 3 3 } vshuffle :> c2
    factors' { 3 3 2 3 } vshuffle :> c3
    float-4{ 0.0 0.0 0.0 1.0 } :> c4

    c1 c rows>> set-first
    c2 c rows>> set-second
    c3 c rows>> set-third
    c4 c rows>> set-fourth

    c ;

: ortho-matrix4 ( factors -- matrix )
    float-4{ 1.0 1.0 1.0 1.0 } swap v/ scale-matrix4 ; inline

TYPED:: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
    matrix4 (struct) :> c

    float-4{ 0.0 0.0 0.0 1.0 } :> c4
    { t t t f } offset c4 v? :> offset'
    offset' { 3 3 3 0 } vshuffle { t f f t } vmask :> c1
    offset' { 3 3 3 1 } vshuffle { f t f t } vmask :> c2
    offset' { 3 3 3 2 } vshuffle { f f t t } vmask :> c3

    c1 c rows>> set-first
    c2 c rows>> set-second
    c3 c rows>> set-third
    c4 c rows>> set-fourth

    c ;

! TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
!     matrix4 (struct) :> c
!     float-4{  1.0 -1.0  1.0 0.0 } :> triangle-sign
! 
!     theta cos float-4-with :> cc
!     theta sin float-4-with :> ss
!     1.0 float-4-with :> ones
!     ones cc v- :> 1-c
!     axis axis v* :> axis2
! 
!     axis2 cc ones axis2 v- v* v+ ones
!     [ { t t t f } ] 2dip v? :> diagonal
! 
!     axis { 0 0 1 3 } vshuffle axis { 1 2 2 3 } vshuffle v* 1-c v* :> triangle-a
!     ss { 2 1 0 3 } vshuffle triangle-sign * :> triangle-b
!     triangle-a triangle-b + :> triangle-lo
!     triangle-a triangle-b - :> triangle-hi
! 
!     ... ;
! !   x*x + c*(1.0 - x*x)   x*y*(1.0 - c) - s*z   x*z*(1.0 - c) + s*y   0
! !   x*y*(1.0 - c) + s*z   y*y + c*(1.0 - y*y)   y*z*(1.0 - c) - s*x   0
! !   x*z*(1.0 - c) - s*y   y*z*(1.0 - c) + s*x   z*z + c*(1.0 - z*z)   0
! !   0                     0                     0                     1
! 
! TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )