diff --git a/extra/math/matrices/simd/simd-tests.factor b/extra/math/matrices/simd/simd-tests.factor index 5bd61adefd..60b37f5371 100644 --- a/extra/math/matrices/simd/simd-tests.factor +++ b/extra/math/matrices/simd/simd-tests.factor @@ -53,6 +53,19 @@ IN: math.matrices.simd.tests 1.0e-7 m~ ] unit-test +[ t ] [ + float-4{ 0.0 1.0 0.0 1.0 } pi 1/2. * rotation-matrix4 + S{ matrix4 f + float-4-array{ + float-4{ 0.0 0.0 1.0 0.0 } + float-4{ 0.0 1.0 0.0 0.0 } + float-4{ -1.0 0.0 0.0 0.0 } + float-4{ 0.0 0.0 0.0 1.0 } + } + } + 1.0e-7 m~ +] unit-test + [ S{ matrix4 f float-4-array{ diff --git a/extra/math/matrices/simd/simd.factor b/extra/math/matrices/simd/simd.factor index bc213fec3a..fc6c2a03f8 100644 --- a/extra/math/matrices/simd/simd.factor +++ b/extra/math/matrices/simd/simd.factor @@ -95,6 +95,17 @@ TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 ) c set-rows ; +TYPED:: v.m4 ( a: float-4 b: matrix4 -- c: float-4 ) + b rows :> b4 :> b3 :> b2 :> b1 + + a first b1 n*v + a second b2 n*v v+ + a third b3 n*v v+ + a fourth b4 n*v v+ ; + +TYPED:: m4.v ( a: matrix4 b: float-4 -- c: float-4 ) + a rows [ b v. ] 4 napply float-4-boa ; + CONSTANT: identity-matrix4 S{ matrix4 f float-4-array{ @@ -165,17 +176,17 @@ TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 ) axis2 cc ones axis2 v- v* v+ :> diagonal - axis { 0 0 1 3 } vshuffle axis { 1 2 2 3 } vshuffle v* 1-c v* + axis { 1 0 0 3 } vshuffle axis { 2 2 1 3 } vshuffle v* 1-c v* float-4{ t t t f } vmask :> triangle-a - ss { 2 1 0 3 } vshuffle triangle-sign v* :> triangle-b + ss axis v* triangle-sign v* :> triangle-b triangle-a triangle-b v+ :> triangle-lo triangle-a triangle-b v- :> triangle-hi diagonal scale-matrix4 :> diagonal-m - triangle-hi { 3 0 1 3 } vshuffle - triangle-hi { 3 3 2 3 } vshuffle triangle-lo { 0 3 3 3 } vshuffle v+ - triangle-lo { 1 2 3 3 } vshuffle + triangle-hi { 3 2 1 3 } vshuffle + triangle-hi { 3 3 0 3 } vshuffle triangle-lo { 2 3 3 3 } vshuffle v+ + triangle-lo { 1 0 3 3 } vshuffle float-4 new triangle-m set-rows drop