basis/math/vectors/vectors.factor

   1 ! Copyright (C) 2005, 2009 Slava Pestov.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: arrays alien.c-types assocs kernel sequences math math.functions
   4 hints math.order math.libm fry combinators byte-arrays accessors
   5 locals ;
   6 QUALIFIED-WITH: alien.c-types c
   7 IN: math.vectors
   8
   9 MIXIN: simd-128
  10 MIXIN: simd-256
  11
  12 GENERIC: element-type ( obj -- c-type )
  13 M: object element-type drop f ; inline
  14
  15 : vneg ( u -- v ) [ neg ] map ;
  16
  17 : v+n ( u n -- v ) [ + ] curry map ;
  18 : n+v ( n u -- v ) [ + ] with map ;
  19 : v-n ( u n -- v ) [ - ] curry map ;
  20 : n-v ( n u -- v ) [ - ] with map ;
  21
  22 : v*n ( u n -- v ) [ * ] curry map ;
  23 : n*v ( n u -- v ) [ * ] with map ;
  24 : v/n ( u n -- v ) [ / ] curry map ;
  25 : n/v ( n u -- v ) [ / ] with map ;
  26
  27 : v+   ( u v -- w ) [ + ] 2map ;
  28 : v-   ( u v -- w ) [ - ] 2map ;
  29 : [v-] ( u v -- w ) [ [-] ] 2map ;
  30 : v*   ( u v -- w ) [ * ] 2map ;
  31 : v/   ( u v -- w ) [ / ] 2map ;
  32 : vmax ( u v -- w ) [ max ] 2map ;
  33 : vmin ( u v -- w ) [ min ] 2map ;
  34
  35 : v+- ( u v -- w )
  36     [ t ] 2dip
  37     [ [ not ] 2dip pick [ + ] [ - ] if ] 2map
  38     nip ;
  39
  40 <PRIVATE
  41
  42 : 2saturate-map ( u v quot -- w )
  43     pick element-type '[ @ _ c-type-clamp ] 2map ; inline
  44
  45 PRIVATE>
  46
  47 : vs+ ( u v -- w ) [ + ] 2saturate-map ;
  48 : vs- ( u v -- w ) [ - ] 2saturate-map ;
  49 : vs* ( u v -- w ) [ * ] 2saturate-map ;
  50
  51 : vabs ( u -- v ) [ abs ] map ;
  52 : vsqrt ( u -- v ) [ >float fsqrt ] map ;
  53
  54 <PRIVATE
  55
  56 : fp-bitwise-op ( x y seq quot -- z )
  57     swap element-type {
  58         { c:double [ [ [ double>bits ] bi@ ] dip call bits>double ] }
  59         { c:float  [ [ [ float>bits ] bi@ ] dip call bits>float   ] }
  60         [ drop call ]
  61     } case ; inline
  62
  63 : fp-bitwise-unary ( x seq quot -- z )
  64     swap element-type {
  65         { c:double [ [ double>bits ] dip call bits>double ] }
  66         { c:float  [ [ float>bits  ] dip call bits>float  ] }
  67         [ drop call ]
  68     } case ; inline
  69
  70 : element>bool ( x seq -- ? )
  71     element-type [ [ f ] when-zero ] when ; inline
  72
  73 : bitandn ( x y -- z ) [ bitnot ] dip bitand ; inline
  74
  75 GENERIC: new-underlying ( underlying seq -- seq' )
  76
  77 : change-underlying ( seq quot -- seq' )
  78     '[ underlying>> @ ] keep new-underlying ; inline
  79
  80 PRIVATE>
  81
  82 : vbitand ( u v -- w ) over '[ _ [ bitand ] fp-bitwise-op ] 2map ;
  83 : vbitandn ( u v -- w ) over '[ _ [ bitandn ] fp-bitwise-op ] 2map ;
  84 : vbitor ( u v -- w ) over '[ _ [ bitor ] fp-bitwise-op ] 2map ;
  85 : vbitxor ( u v -- w ) over '[ _ [ bitxor ] fp-bitwise-op ] 2map ;
  86 : vbitnot ( u -- w ) dup '[ _ [ bitnot ] fp-bitwise-unary ] map ;
  87
  88 :: vbroadcast ( u n -- v ) u length n u nth <repetition> u like ;
  89
  90 : vshuffle-elements ( u perm -- v )
  91     swap [ '[ _ nth ] ] keep map-as ;
  92
  93 : vshuffle-bytes ( u perm -- v )
  94     underlying>> [
  95         swap [ '[ _ nth ] ] keep map-as
  96     ] curry change-underlying ;
  97
  98 GENERIC: vshuffle ( u perm -- v )
  99 M: array vshuffle ( u perm -- v )
 100     vshuffle-elements ; inline
 101 M: simd-128 vshuffle ( u perm -- v )
 102     vshuffle-bytes ; inline
 103
 104 : vlshift ( u n -- w ) '[ _ shift ] map ;
 105 : vrshift ( u n -- w ) neg '[ _ shift ] map ;
 106
 107 : hlshift ( u n -- w ) '[ _ <byte-array> prepend 16 head ] change-underlying ;
 108 : hrshift ( u n -- w ) '[ _ <byte-array> append 16 tail* ] change-underlying ;
 109
 110 : (vmerge-head) ( u v -- h )
 111     over length 2 /i '[ _ head-slice ] bi@ [ zip ] keep concat-as ;
 112 : (vmerge-tail) ( u v -- t )
 113     over length 2 /i '[ _ tail-slice ] bi@ [ zip ] keep concat-as ;
 114
 115 : (vmerge) ( u v -- h t )
 116     [ (vmerge-head) ] [ (vmerge-tail) ] 2bi ; inline
 117
 118 : vmerge ( u v -- w ) [ zip ] keep concat-as ;
 119
 120 : vand ( u v -- w )  over '[ [ _ element>bool ] bi@ and ] 2map ;
 121 : vandn ( u v -- w ) over '[ [ _ element>bool ] bi@ [ not ] dip and ] 2map ;
 122 : vor  ( u v -- w )  over '[ [ _ element>bool ] bi@ or  ] 2map ;
 123 : vxor ( u v -- w )  over '[ [ _ element>bool ] bi@ xor ] 2map ;
 124 : vnot ( u -- w )    dup '[ _ element>bool not ] map ;
 125
 126 : vall? ( v -- ? ) dup '[ _ element>bool ] all? ;
 127 : vany? ( v -- ? ) dup '[ _ element>bool ] any? ;
 128 : vnone? ( v -- ? ) dup '[ _ element>bool not ] all? ;
 129
 130 : v<  ( u v -- w ) [ <   ] 2map ;
 131 : v<= ( u v -- w ) [ <=  ] 2map ;
 132 : v>= ( u v -- w ) [ >=  ] 2map ;
 133 : v>  ( u v -- w ) [ >   ] 2map ;
 134 : vunordered? ( u v -- w ) [ unordered? ] 2map ;
 135 : v=  ( u v -- w ) [ =   ] 2map ;
 136
 137 : v? ( mask true false -- w )
 138     [ vand ] [ vandn ] bi-curry* bi vor ; inline
 139
 140 : vfloor    ( u -- v ) [ floor ] map ;
 141 : vceiling  ( u -- v ) [ ceiling ] map ;
 142 : vtruncate ( u -- v ) [ truncate ] map ;
 143
 144 : vsupremum ( seq -- vmax ) [ ] [ vmax ] map-reduce ;
 145 : vinfimum ( seq -- vmin ) [ ] [ vmin ] map-reduce ;
 146
 147 : v. ( u v -- x ) [ conjugate * ] [ + ] 2map-reduce ;
 148 : norm-sq ( v -- x ) [ absq ] [ + ] map-reduce ;
 149 : norm ( v -- x ) norm-sq sqrt ;
 150 : normalize ( u -- v ) dup norm v/n ;
 151
 152 : distance ( u v -- x ) [ - absq ] [ + ] 2map-reduce sqrt ;
 153
 154 : set-axis ( u v axis -- w )
 155     [ [ zero? 2over ? ] dip swap nth ] map-index 2nip ;
 156
 157 <PRIVATE
 158
 159 : 2tetra@ ( p q r s t u v w quot -- )
 160     dup [ [ 2bi@ ] curry 4dip ] dip 2bi@ ; inline
 161
 162 PRIVATE>
 163
 164 : trilerp ( aaa baa aba bba aab bab abb bbb {t,u,v} -- a_tuv )
 165     [ first lerp ] [ second lerp ] [ third lerp ] tri-curry
 166     [ 2tetra@ ] [ 2bi@ ] [ call ] tri* ;
 167
 168 : bilerp ( aa ba ab bb {t,u} -- a_tu )
 169     [ first lerp ] [ second lerp ] bi-curry
 170     [ 2bi@ ] [ call ] bi* ;
 171
 172 : vlerp ( a b t -- a_t )
 173     [ lerp ] 3map ;
 174
 175 : vnlerp ( a b t -- a_t )
 176     [ lerp ] curry 2map ;
 177
 178 : vbilerp ( aa ba ab bb {t,u} -- a_tu )
 179     [ first vnlerp ] [ second vnlerp ] bi-curry
 180     [ 2bi@ ] [ call ] bi* ;
 181
 182 : v~ ( a b epsilon -- ? )
 183     [ ~ ] curry 2all? ;
 184
 185 HINTS: vneg { array } ;
 186 HINTS: norm-sq { array } ;
 187 HINTS: norm { array } ;
 188 HINTS: normalize { array } ;
 189 HINTS: distance { array array } ;
 190
 191 HINTS: n*v { object array } ;
 192 HINTS: v*n { array object } ;
 193 HINTS: n/v { array } ;
 194 HINTS: v/n { array object } ;
 195
 196 HINTS: v+ { array array } ;
 197 HINTS: v- { array array } ;
 198 HINTS: v* { array array } ;
 199 HINTS: v/ { array array } ;
 200 HINTS: vmax { array array } ;
 201 HINTS: vmin { array array } ;
 202 HINTS: v. { array array } ;
 203
 204 HINTS: vlerp { array array array } ;
 205 HINTS: vnlerp { array array object } ;
 206
 207 HINTS: bilerp { object object object object array } ;
 208 HINTS: trilerp { object object object object object object object object array } ;