extra/boids/simulation/simulation.factor

   1 ! Copyright (C) 2008 Eduardo Cavazos.
   2 ! Copyright (C) 2011 Anton Gorenko.
   3 ! See http://factorcode.org/license.txt for BSD license.
   4 USING: accessors arrays combinators.short-circuit kernel
   5 locals math math.vectors random sequences ;
   6 IN: boids.simulation
   7
   8 CONSTANT: width 512
   9 CONSTANT: height 512
  10
  11 TUPLE: behaviour
  12     { weight float }
  13     { radius float }
  14     { angle-cos float } ;
  15
  16 TUPLE: boid
  17     { pos array }
  18     { vel array } ;
  19
  20 C: <boid> boid
  21
  22 : vsum ( vecs -- v )
  23     { 0.0 0.0 } [ v+ ] reduce ; inline
  24
  25 : vavg ( vecs -- v )
  26     [ vsum ] [ length ] bi v/n ; inline
  27
  28 : in-radius? ( self other radius -- ? )
  29     [ [ pos>> ] bi@ distance ] dip <= ; inline
  30
  31 : angle-between ( u v -- angle )
  32     [ normalize ] bi@ v. ; inline
  33
  34 : relative-position ( self other -- v )
  35     swap [ pos>> ] bi@ v- ; inline
  36
  37 :: relative-angle ( self other -- angle )
  38     self other relative-position
  39     self vel>> angle-between ; inline
  40
  41 : in-view? ( self other angle-cos -- ? )
  42     [ relative-angle ] dip >= ; inline
  43
  44 :: within-neighborhood? ( self other behaviour -- ? )
  45     self other {
  46         [ eq? not ]
  47         [ behaviour radius>> in-radius? ]
  48         [ behaviour angle-cos>> in-view? ]
  49     } 2&& ; inline
  50
  51 :: neighbors ( boid boids behaviour -- neighbors )
  52     boid boids [ behaviour within-neighborhood? ] with filter ;
  53
  54
  55 GENERIC: force ( neighbors boid behaviour -- force )
  56
  57 :: (force) ( boid boids behaviour -- force )
  58     boid boids behaviour neighbors
  59     [ { 0.0 0.0 } ] [ boid behaviour force ] if-empty ;
  60
  61 : wrap-pos ( pos -- pos )
  62     width height [ 1 - ] bi@ 2array
  63     [ [ + ] keep mod ] 2map ;
  64
  65 :: simulate ( boids behaviours dt -- boids )
  66     boids [| boid |
  67         boid boids behaviours
  68         [ [ (force) ] keep weight>> v*n ] with with map vsum :> a
  69
  70         boid vel>> a dt v*n v+ normalize :> vel
  71         boid pos>> vel dt v*n v+ wrap-pos :> pos
  72
  73         pos vel <boid>
  74     ] map ;
  75
  76 : random-boids ( count -- boids )
  77     [
  78         width height [ random ] bi@ 2array
  79         2 [ 0 1 normal-random-float ] replicate
  80         <boid>
  81     ] replicate ;
  82
  83 TUPLE: cohesion < behaviour ;
  84 TUPLE: alignment < behaviour ;
  85 TUPLE: separation < behaviour ;
  86
  87 C: <cohesion> cohesion
  88 C: <alignment> alignment
  89 C: <separation> separation
  90
  91 M: cohesion force ( neighbors boid behaviour -- force )
  92     drop [ [ pos>> ] map vavg ] [ pos>> ] bi* v- normalize ;
  93
  94 M: alignment force ( neighbors boid behaviour -- force )
  95     2drop [ vel>> ] map vsum normalize ;
  96
  97 M:: separation force ( neighbors boid behaviour -- force )
  98     behaviour radius>> :> r
  99     boid pos>> neighbors
 100     [ pos>> v- [ normalize ] [ r v/n ] bi v- ] with map vsum ;
 101