extra/springies/springies.factor

   1
   2 USING: kernel combinators sequences arrays math math.vectors
   3        generalizations vars accessors math.physics.vel ;
   4
   5 IN: springies
   6
   7 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   8
   9 : scalar-projection ( a b -- n ) [ v. ] [ nip norm ] 2bi / ;
  10
  11 : vector-projection ( a b -- vec )
  12   [ nip normalize ] [ scalar-projection ] 2bi v*n ;
  13
  14 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  15
  16 VAR: nodes
  17 VAR: springs
  18 VAR: time-slice
  19 VAR: world-size
  20
  21 : world-width ( -- width ) world-size> first ;
  22
  23 : world-height ( -- height ) world-size> second ;
  24
  25 VAR: gravity
  26
  27 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  28 ! node
  29 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  30
  31 TUPLE: node < vel mass elas force ;
  32
  33 C: <node> node
  34
  35 : node-vel ( node -- vel ) vel>> ;
  36
  37 : set-node-vel ( vel node -- ) swap >>vel drop ;
  38
  39 : pos-x ( node -- x ) pos>> first ;
  40 : pos-y ( node -- y ) pos>> second ;
  41 : vel-x ( node -- y ) vel>> first ;
  42 : vel-y ( node -- y ) vel>> second ;
  43
  44 : >>pos-x ( node x -- node ) over pos>> set-first ;
  45 : >>pos-y ( node y -- node ) over pos>> set-second ;
  46 : >>vel-x ( node x -- node ) over vel>> set-first ;
  47 : >>vel-y ( node y -- node ) over vel>> set-second ;
  48
  49 : apply-force ( node vec -- ) over force>> v+ >>force drop ;
  50
  51 : reset-force ( node -- node ) 0 0 2array >>force ;
  52
  53 : node-id ( id -- node ) 1- nodes> nth ;
  54
  55 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  56 ! spring
  57 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  58
  59 TUPLE: spring rest-length k damp node-a node-b ;
  60
  61 C: <spring> spring
  62
  63 : end-points ( spring -- b-pos a-pos )
  64   [ node-b>> pos>> ] [ node-a>> pos>> ] bi ;
  65
  66 : spring-length ( spring -- length ) end-points v- norm ;
  67
  68 : stretch-length ( spring -- length )
  69   [ spring-length ] [ rest-length>> ] bi - ;
  70
  71 : dir ( spring -- vec ) end-points v- normalize ;
  72
  73 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  74 ! Hooke
  75 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  76 !
  77 ! F = -kx
  78 !
  79 ! k :: spring constant
  80 ! x :: distance stretched beyond rest length
  81 !
  82 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  83
  84 : hooke-force-mag ( spring -- mag ) [ k>> ] [ stretch-length ] bi * ;
  85
  86 : hooke-force ( spring -- force ) [ dir ] [ hooke-force-mag ] bi v*n ;
  87
  88 : hooke-forces ( spring -- a b ) hooke-force dup vneg ;
  89
  90 : act-on-nodes-hooke ( spring -- )
  91   [ node-a>> ] [ node-b>> ] [ ] tri hooke-forces swapd
  92   apply-force
  93   apply-force ;
  94
  95 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  96 ! damping
  97 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  98 !
  99 ! F = -bv
 100 !
 101 ! b :: Damping constant
 102 ! v :: Velocity
 103 !
 104 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 105
 106 ! : damping-force-a ( spring -- vec )
 107 !   [ spring-node-a node-vel ] [ spring-damp ] bi v*n vneg ;
 108
 109 ! : damping-force-b ( spring -- vec )
 110 !   [ spring-node-b node-vel ] [ spring-damp ] bi v*n vneg ;
 111
 112 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 113
 114 : relative-velocity-a ( spring -- vel )
 115   [ node-a>> vel>> ] [ node-b>> vel>> ] bi v- ;
 116
 117 : unit-vec-b->a ( spring -- vec )
 118   [ node-a>> pos>> ] [ node-b>> pos>> ] bi v- ;
 119
 120 : relative-velocity-along-spring-a ( spring -- vel )
 121   [ relative-velocity-a ] [ unit-vec-b->a ] bi vector-projection ;
 122
 123 : damping-force-a ( spring -- vec )
 124   [ relative-velocity-along-spring-a ] [ damp>> ] bi v*n vneg ;
 125
 126 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 127
 128 : relative-velocity-b ( spring -- vel )
 129   [ node-b>> vel>> ] [ node-a>> vel>> ] bi v- ;
 130
 131 : unit-vec-a->b ( spring -- vec )
 132   [ node-b>> pos>> ] [ node-a>> pos>> ] bi v- ;
 133
 134 : relative-velocity-along-spring-b ( spring -- vel )
 135   [ relative-velocity-b ] [ unit-vec-a->b ] bi vector-projection ;
 136
 137 : damping-force-b ( spring -- vec )
 138   [ relative-velocity-along-spring-b ] [ damp>> ] bi v*n vneg ;
 139
 140 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 141
 142 : act-on-nodes-damping ( spring -- )
 143   dup
 144   [ node-a>> ] [ damping-force-a ] bi apply-force
 145   [ node-b>> ] [ damping-force-b ] bi apply-force ;
 146
 147 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 148
 149 : below? ( node -- ? ) pos-y 0 < ;
 150
 151 : above? ( node -- ? ) pos-y world-height >= ;
 152
 153 : beyond-left? ( node -- ? ) pos-x 0 < ;
 154
 155 : beyond-right? ( node -- ? ) pos-x world-width >= ;
 156
 157 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 158
 159 : bounce-top ( node -- )
 160   world-height 1- >>pos-y
 161   dup [ vel-y ] [ elas>> ] bi * neg >>vel-y
 162   drop ;
 163
 164 : bounce-bottom ( node -- )
 165   0 >>pos-y
 166   dup [ vel-y ] [ elas>> ] bi * neg >>vel-y
 167   drop ;
 168
 169 : bounce-left ( node -- )
 170   0 >>pos-x
 171   dup [ vel-x ] [ elas>> ] bi * neg >>vel-x
 172   drop ;
 173
 174 : bounce-right ( node -- )
 175   world-width 1- >>pos-x
 176   dup [ vel-x ] [ elas>> ] bi * neg >>vel-x
 177   drop ;
 178
 179 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 180
 181 : handle-bounce ( node -- )
 182   { { [ dup above? ]        [ bounce-top ] }
 183     { [ dup below? ]        [ bounce-bottom ] }
 184     { [ dup beyond-left? ]  [ bounce-left ] }
 185     { [ dup beyond-right? ] [ bounce-right ] }
 186     { [ t ]                 [ drop ] } }
 187   cond ;
 188
 189 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 190
 191 : act-on-nodes ( spring -- )
 192   dup
 193   act-on-nodes-hooke
 194   act-on-nodes-damping ;
 195
 196 ! : act-on-nodes ( spring -- ) act-on-nodes-hooke ;
 197
 198 : loop-over-springs ( -- ) springs> [ act-on-nodes ] each ;
 199
 200 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 201
 202 : apply-gravity ( node -- ) { 0 -9.8 } apply-force ;
 203
 204 : do-gravity ( -- ) gravity> [ nodes> [ apply-gravity ] each ] when ;
 205
 206 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 207
 208 ! F = ma
 209
 210 : calc-acceleration ( node -- vec ) [ force>> ] [ mass>> ] bi v/n ;
 211
 212 : new-vel ( node -- vel )
 213   [ vel>> ] [ calc-acceleration time-slice> v*n ] bi v+ ;
 214
 215 : new-pos ( node -- pos ) [ pos>> ] [ vel>> time-slice> v*n ] bi v+ ;
 216
 217 : iterate-node ( node -- )
 218   dup new-pos >>pos
 219   dup new-vel >>vel
 220   reset-force
 221   handle-bounce ;
 222
 223 : iterate-nodes ( -- ) nodes> [ iterate-node ] each ;
 224
 225 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 226
 227 : iterate-system ( -- ) do-gravity loop-over-springs iterate-nodes ;
 228
 229 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 230 ! Reading xspringies data files
 231 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 232
 233 : mass ( id x y x-vel y-vel mass elas -- )
 234   node new
 235     swap >>elas
 236     swap >>mass
 237     -rot 2array >>vel
 238     -rot 2array >>pos
 239     0 0  2array >>force
 240   nodes> swap suffix >nodes
 241   drop ;
 242
 243 : spng ( id id-a id-b k damp rest-length -- )
 244    spring new
 245      swap >>rest-length
 246      swap >>damp
 247      swap >>k
 248      swap node-id >>node-b
 249      swap node-id >>node-a
 250    springs> swap suffix >springs
 251    drop ;