quaterion to matrix4 conversion

basis/math/quaternions/quaternions.factor

@@ -9,15 +9,23 @@ IN: math.quaternions
 : q- ( u v -- u-v )
     v- ; inline
 
+<PRIVATE
+
+GENERIC: (q*sign) ( q -- q' )
+M: object (q*sign) { -1 1 1 1 } v* ; inline
+
+PRIVATE>
+
 : q* ( u v -- u*v )
     {
         [ [ { 1 0 0 0 } vshuffle ] [ { 1 1 2 3 } vshuffle ] bi* v*    ]
         [ [ { 2 1 2 3 } vshuffle ] [ { 2 0 0 0 } vshuffle ] bi* v* v+ ]
         [ [ { 3 2 3 1 } vshuffle ] [ { 3 3 1 2 } vshuffle ] bi* v* v+ ]
         [ [ { 0 3 1 2 } vshuffle ] [ { 0 2 3 1 } vshuffle ] bi* v* v- ]
-    } 2cleave { -1 1 1 1 } v* ; inline
+    } 2cleave (q*sign) ; inline
 
-: qconjugate ( u -- u' )
+GENERIC: qconjugate ( u -- u' )
+M: object qconjugate ( u -- u' )
     { 1 -1 -1 -1 } v* ; inline
 
 : qrecip ( u -- 1/u )
@@ -59,4 +67,3 @@ PRIVATE>
 
 : euler ( phi theta psi -- q )
     { } euler-like ; inline
-

extra/math/matrices/simd/simd-tests.factor

@@ -238,3 +238,42 @@ IN: math.matrices.simd.tests
     float-4{ 0.5 0.5 0.5 1.0 } scale-matrix4 m4.
     float-4{ 2.0 3.0 4.0 1.0 } m4.v
 ] unit-test
+
+[
+    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 }
+        }
+    }
+] [
+    float-4{ 1.0 0.0 0.0 0.0 } q>matrix4
+] unit-test
+
+[ t ] [
+    pi 0.5 * 0.0 0.0 euler4 q>matrix4
+    S{ matrix4 f
+        float-4-array{
+            float-4{ 1.0  0.0  0.0  0.0 }
+            float-4{ 0.0  0.0  1.0  0.0 }
+            float-4{ 0.0 -1.0  0.0  0.0 }
+            float-4{ 0.0  0.0  0.0  1.0 }
+        }
+    }
+    1.0e-7 m~ 
+] unit-test
+
+[ t ] [
+    0.0 pi 0.25 * 0.0 euler4 q>matrix4
+    S{ matrix4 f
+        float-4-array{
+            float-4{ $[ 1/2. sqrt ] 0.0 $[ 1/2. sqrt neg ] 0.0 }
+            float-4{ 0.0            1.0 0.0                0.0 }
+            float-4{ $[ 1/2. sqrt ] 0.0 $[ 1/2. sqrt     ] 0.0 }
+            float-4{ 0.0            0.0 0.0                1.0 }
+        }
+    }
+    1.0e-7 m~ 
+] unit-test

extra/math/matrices/simd/simd.factor

@@ -1,8 +1,10 @@
 ! (c)Joe Groff bsd license
 USING: accessors classes.struct fry generalizations kernel locals
 math math.combinatorics math.functions math.matrices.simd math.vectors
-math.vectors.simd sequences sequences.private specialized-arrays
+math.vectors.simd math.quaternions sequences sequences.private specialized-arrays
 typed ;
+FROM: sequences.private => nth-unsafe ;
+FROM: math.quaternions.private => (q*sign) ;
 QUALIFIED-WITH: alien.c-types c
 SPECIALIZED-ARRAY: float-4
 IN: math.matrices.simd
@@ -23,10 +25,7 @@ M: matrix4 new-sequence 2drop matrix4 (struct) ; inline
 
 :: set-columns ( c1 c2 c3 c4 c -- c )
     c columns>> :> columns
-    c1 columns set-first
+    c1 c2 c3 c4 columns 4 set-firstn-unsafe
-    c2 columns set-second
-    c3 columns set-third
-    c4 columns set-fourth
     c ; inline
 
 : make-matrix4 ( quot: ( -- c1 c2 c3 c4 ) -- c )
@@ -151,12 +150,24 @@ TYPED: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
         ] dip
     ] make-matrix4 ;
 
+:: (rotation-matrix4) ( diagonal triangle-hi triangle-lo -- matrix )
+    matrix4 (struct) :> triangle-m
+    diagonal scale-matrix4 :> diagonal-m
+
+    triangle-hi { 3 2 1 3 } vshuffle
+    triangle-hi { 3 3 0 3 } vshuffle triangle-lo { 2 3 3 3 } vshuffle vbitor
+                                     triangle-lo { 1 0 3 3 } vshuffle
+    float-4 new
+
+    triangle-m set-columns drop
+
+    diagonal-m triangle-m m4+ ; inline
+
 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
     !   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
-    matrix4 (struct) :> triangle-m
     theta cos :> c
     theta sin :> s
 
@@ -176,17 +187,8 @@ TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
     triangle-a triangle-b v+ :> triangle-lo
     triangle-a triangle-b v- :> triangle-hi
 
-    diagonal scale-matrix4 :> diagonal-m
+    diagonal triangle-hi triangle-lo (rotation-matrix4) ;
-
+    
-    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-columns drop
-
-    diagonal-m triangle-m m4+ ;
-
 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
     [
         near near near far + 2 near far * * float-4-boa ! num
@@ -200,3 +202,30 @@ TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4
         [ negone (vmerge) ] bi*
     ] make-matrix4 ;
 
+! interface with quaternions
+M: float-4 (q*sign)
+    float-4{ -0.0  0.0  0.0  0.0 } vbitxor ; inline
+M: float-4 qconjugate
+    float-4{  0.0 -0.0 -0.0 -0.0 } vbitxor ; inline
+
+: euler4 ( phi theta psi -- q )
+    float-4{ 0 0 0 0 } euler-like ; inline
+
+TYPED:: q>matrix4 ( q: float-4 -- matrix: matrix4 )
+    !   a*a + b*b - c*c - d*d  2*b*c - 2*a*d          2*b*d + 2*a*c          0
+    !   2*b*c + 2*a*d          a*a - b*b + c*c - d*d  2*c*d - 2*a*b          0
+    !   2*b*d - 2*a*c          2*c*d + 2*a*b          a*a - b*b - c*c + d*d  0
+    !   0                      0                      0                      1
+    q { 2 1 1 3 } vshuffle  q { 3 3 2 3 } vshuffle  v*  :> triangle-a
+    q { 0 0 0 3 } vshuffle  q { 1 2 3 3 } vshuffle  v*  :> triangle-b
+
+    triangle-a float-4{ 2.0 2.0 2.0 0.0 } v*  triangle-b float-4{ -2.0 2.0 -2.0 0.0 } v*
+    [ v- ] [ v+ ] 2bi :> ( triangle-hi triangle-lo )
+
+    q q v* first4 {
+        [ [ + ] [ - ] [ - ] tri* ]
+        [ [ - ] [ + ] [ - ] tri* ]
+        [ [ - ] [ - ] [ + ] tri* ]
+    } 4 ncleave 1.0 float-4-boa :> diagonal
+
+    diagonal triangle-hi triangle-lo (rotation-matrix4) ;