steve
/
factor
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

quaterion to matrix4 conversion

db4
Joe Groff 2010-02-04 11:25:45 -08:00
parent 4ba8c6e0b5
commit 2381e9bf24
3 changed files with 95 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
basis/math/quaternions/quaternions.factor
View File

@ -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

39
extra/math/matrices/simd/simd-tests.factor
View File

@ -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

63
extra/math/matrices/simd/simd.factor
View File

@ -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
c2 columns set-second
c3 columns set-third
c4 columns set-fourth
c1 c2 c3 c4 columns 4 set-firstn-unsafe
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
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+ ;
diagonal triangle-hi triangle-lo (rotation-matrix4) ;
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) ;