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