;;
;; Swarm simulation
;;
;; Simulation of the swarm of flies. Written as a test of concepts of complex systems.
;;
;; Ideas borrowed from the great C and Java program of Alex Vulliamy (and Jeff Cragg):
;; - http://www.1stpm.org/alex
;; - http://www.1stpm.org/alex/moreflies.html
;;
;;
;; Frantisek Sodomka, Dec 01 2005
;; Updated for 10.1 L.M. Nov 27, 2009
;;


;
; Versions:
;
; 0.00 - start
; 0.01, 0.02
;   - fixed major errors when calculating distance to neighbours
;   - tests of different ways and rules
;   - added rules for movement
; 0.03
;   - velocity doesn't disappear from one stage to the other
;     => first working version (finally :)
; 0.04
;   - added limit for velocity - otherwise in some cases system becomes unstable
;     => every follower has to go to the max or under
; 0.05
;   - added base + random velocity for the leader
;   - added random velocity vector for each follower
;   - changes in parameters
; 0.06
;   - accelerations can be changed randomly
;   - leader/followers are lines, not ovals
;   - leader has a better logic around the edges
; 0.07
;   - it doesn't show the error when closing the window (=> it's a little bit slower than 0.06)
;   - window has a different size in x and y (MX MY)
;   - Tk code clean up
;   - can be run by both newlisp-tk and Tk wrapper (exits correctly)
;
; 0.08
;   - updated for 10.1


;
; Global variables and constants
;

; version
(constant 'VER "0.08")

; PI
(constant 'PI (mul 2 (acos 0)))


################### OBJECTS ###################

; width/length
(constant 'W 12)

; thickness
(constant 'TH 3)

; color for leader/followers
(constant 'COLORS '("red" "orange"))

; sleep between drawings [ms]
(constant 'SLEEP-TIME 30)


################## FOLLOWERS ##################

; how many
(constant 'N 50)

; how often to random the accelerations (in iterations)
(constant 'RND-ACC-ITER 150)

; accelerations to the leader, 2 neighbours and the center of the swarm (base + random addition)
(constant 'ACC-L-BASE 0.015)
(constant 'ACC-L-ADD  0.020)

(constant 'ACC-N-BASE 0.007)
(constant 'ACC-N-ADD  0.010)

(constant 'ACC-CNT-BASE 0.005)
(constant 'ACC-CNT-ADD  0.007)

; OK
;(setq 'ACC-L   0.025)
;(setq 'ACC-N   0.008)
;(setq 'ACC-CNT 0.05)


; distance for deciding between attraction/repulsion for neighbours
(constant 'DIST-N   10)

; maximum velocity for the follower
(constant 'F-VEL-MAX 30)

; random velocity for the follower
(constant 'F-VEL-RND 6)


################### LEADER ####################

; maximum change of angle for the leader
(constant 'L-ANG-MAX (div PI 8))

; velocity of the leader - base + random of addition
(constant 'L-VEL-BASE 12)
(constant 'L-VEL-ADD  5)


##################### TK ######################

; window
(constant 'MX (int (mul 0.8 (float (tk "set ::SW [winfo screenwidth  .]")))))
(constant 'MY (int (mul 0.8 (float (tk "set ::SH [winfo screenheight .]")))))


#################################################################################
#################################################################################
#################################################################################


; distance for deciding between attraction/repulsion to the center - CURRENTLY NOT USED
;(constant 'DIST-CNT 15)


; Global variables for accelerations
(setq ACC-L   0)
(setq ACC-N   0)
(setq ACC-CNT 0)

;
; Randomize accelerations
;
(define (randomize-accelerations)
  (setq ACC-L   (add ACC-L-BASE   (random 0 ACC-L-ADD)))
  (setq ACC-N   (add ACC-N-BASE   (random 0 ACC-N-ADD)))
  (setq ACC-CNT (add ACC-CNT-BASE (random 0 ACC-CNT-ADD))))


;
; Inicialization of the leader and followers
;
; - format: (x y vx vy)
;
(setq leader    '())
(setq followers '())

(define (random-position)
  (append (map (lambda (x) (+ x W)) (list (rand (- MX (* 2 W))) (rand (- MY (* 2 W))))) '(0 0)))

(define (initial-settings)
  ; positions and velocity
  (setq leader (random-position))
  (setq followers '())
  (dotimes (i N)
    (push (random-position) followers))

  ; accelerations
  (randomize-accelerations))


;
; Initial draw
;
(define (draw-line data color)
  (tk ".w.c create line " (- (data 0) 1) " " (- (data 1) 1) " " (+ (data 0) 1) " " (+ (data 1) 1) " -fill " color " -width " TH))

(define (initial-draw)
  (draw-line leader (COLORS 0))
  (dotimes (i N)
    (draw-line (followers i) (COLORS 1))))


;
; Move one object
;
(define (move ind data)
  (let (x 0 y 0 vx 0 vy 0 r 0)
    (map set '(x y vx vy) data)
    (setq r (div (/ W 2) (max (abs vx) (abs vy))))
    (setq vx (mul r vx))
    (setq vy (mul r vy))
    (tk "if { [winfo exists .w] } { .w.c coords " ind " " (- x vx) " " (- y vy) " " (+ x vx) " " (+ y vy) " }")))

;
; Move all objects to new positions
;
(define (move-all)
  (move 1 leader)
  (dotimes (i N)
    (move (+ i 2) (followers i))))


;
; Angle of the vector (x,y) -> 0 to 2*PI
;
(define (angle x y)
  (let (a (atan2 y x))
    (if (< a 0)
      (add (mul 2 PI) a)
      a)))

;
; Distance [x1,y1] to [x2,y2]
;
(define (distance x1 y1 x2 y2)
  (sqrt (add (pow (sub x1 x2) 2) (pow (sub y1 y2) 2))))


;
; Calculate positions of followers
;
; Rules for followers:
; 1. follow the leader
; 2. get closer/farther to/from the 2 closest neighbours
; 3. get closer to the center of the swarm
; 4. move little bit randomly
;
(define (calculate-followers)
  (let (fxy '() centerx 0 centery 0 lx 0 ly 0 x 0 y 0 vx 0 vy 0 dx 0 dy 0 d 0 dist '() nv '())

    ; followers xs & ys
    (setq fxy (map (lambda (x) (list (x 0) (x 1))) followers))

    ; center
    (setq centerx (div (apply add (map first fxy)) N))
    (setq centery (div (apply add (map (lambda (x) (x 1)) fxy)) N))

    ; leader's x & y
    (setq lx (leader 0))
    (setq ly (leader 1))

    ; new velocity cycle
    (dotimes (i N)
      (map set '(x y vx vy) (followers i))

      ; 1. follow the leader
      (setq dx (sub lx x))
      (setq dy (sub ly y))

      (inc vx (mul ACC-L dx))
      (inc vy (mul ACC-L dy))

      ; 2. get closer/farther to/from the 2 closest neighbours - not optimized!
      (setq dist '())
      (dotimes (j N)
        (if (!= i j)
          (push (list (distance x y ((fxy j) 0) ((fxy j) 1)) j) dist -1)))

      (setq dist (sort dist))

      ; first neighbour
      (setq d (sub ((dist 0) 0) DIST-N))
      (inc vx (mul ACC-N (sgn d) (sub ((fxy ((dist 0) 1)) 0) x)))
      (inc vy (mul ACC-N (sgn d) (sub ((fxy ((dist 0) 1)) 1) y)))

      ; second neighbour
      (setq d (sub ((dist 1) 0) DIST-N))
      (inc vx (mul ACC-N (sgn d) (sub ((fxy ((dist 1) 1)) 0) x)))
      (inc vy (mul ACC-N (sgn d) (sub ((fxy ((dist 1) 1)) 1) y)))

      ; 3. get closer to the center of the swarm
      (inc vx (mul ACC-CNT (sub centerx x)))
      (inc vy (mul ACC-CNT (sub centery y)))

      ;(setq d (sub (distance x y centerx centery) DIST-CNT))
      ;(inc vx (mul ACC-CNT (sgn d) (sub centerx x)))
      ;(inc vy (mul ACC-CNT (sgn d) (sub centery y)))

      ; 4. move little bit randomly
      (inc vx (random (sub F-VEL-RND) (mul 2 F-VEL-RND)))
      (inc vy (random (sub F-VEL-RND) (mul 2 F-VEL-RND)))

      ; New follower's velocity
      (push (list vx vy) nv -1))

    ; refactoring of velocity
    (setq vx (div F-VEL-MAX (apply max (map (lambda (x) (abs (x 0))) nv))))
    (setq vy (div F-VEL-MAX (apply max (map (lambda (x) (abs (x 1))) nv))))

    (setq nv (map (lambda (x) (list (mul vx (x 0)) (mul vy (x 1)))) nv))

    ; new followers
    (setq followers (map (lambda (x y) (list (add (x 0) (y 0)) (add (x 1) (y 1)) (y 0) (y 1))) followers nv))))


;
; Calculate position of the leader
;
(define (calculate-leader)
  (let (x 0 y 0 vx 0 vy 0 ang 0 s 0 b nil)
    (map set '(x y vx vy) leader)

    ; vx, vy
    (setq ang (add (angle vx vy) (random (sub L-ANG-MAX) (mul 2 L-ANG-MAX))))

    ; angle "bending" - left and right
    (if (< (min x (sub MX x)) (mul 5 (add L-VEL-BASE L-VEL-ADD)))
      (begin
        (setq b true)
        (setq s (mul (sgn (sub x (div MX 2))) (sgn (sin ang))))
        (if (< (abs s) 0.01) (setq s 1))
        (inc ang (mul s (div PI 8)))))

    ; angle "bending" - top and bottom
    (if (< (min y (sub MY y)) (mul 5 (add L-VEL-BASE L-VEL-ADD)))
      (if b
        (inc ang (mul s (div PI 8)))  ; in the corner
        (begin
          (setq s (mul (sgn (sub (div MY 2) y)) (sgn (cos ang))))
          (if (< (abs s) 0.01) (setq s 1))
          (inc ang (mul s (div PI 8))))))

    (setq vx (mul (add L-VEL-BASE (random 0 L-VEL-ADD)) (cos ang)))
    (setq vy (mul (add L-VEL-BASE (random 0 L-VEL-ADD)) (sin ang)))

    ; x, y
    (inc x vx)
    (inc y vy)

    ; if something goes wrong, return to the field!
    (if (< x 0)  (setq x 0))
    (if (> x MX) (setq x MX))

    (if (< y 0)  (setq y 0))
    (if (> y MY) (setq y MY))

    ; change leader
    (setq leader (list x y vx vy))))



; Iteration counter
(setq iter-counter 0)

;
; Swarm loop
;
(define (swarm)
  ; loop
  (while (= "1" (tk "winfo exists .w"))
    (calculate-leader)
    (calculate-followers)
    (move-all)
    (sleep SLEEP-TIME)

    ; random acceleration
    (inc iter-counter)
    (if (= iter-counter RND-ACC-ITER)
      (begin
        (randomize-accelerations)
        (setq iter-counter 0))))

  ; exit
  (if NL-TCLTK
    (begin
      (tk "exit")
      (exit))
    (tk "wm deiconify .")))



;
; Run the swarm
;
(define (run)
  (initial-settings)
  (initial-draw)
  (swarm))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


;;
;; Tk help procedures
;;

(tk [text]

  # Create new window
  proc ::create_window {win ttl} {
    if {[winfo exists $win]} { destroy $win }
    toplevel $win
    wm title $win $ttl
    wm resizable $win 0 0
  }

  # Center the window (without showing it moving)
  proc ::center_window {win} {
    wm withdraw $win
    update idletasks

    set w [winfo width $win]
    set h [winfo height $win]
    wm geometry $win "+[expr {($::SW - $w) / 2}]+[expr {($::SH - $h) / 2}]"

    wm deiconify $win
  }

[/text])


;
; Window
;
(tk "wm withdraw .")

(tk "::create_window .w {Swarm v. " VER "}")
(tk "wm geometry .w " MX "x" MY)
(tk "::center_window .w")

(tk "canvas .w.c -width " MX " -height " MY " -background black")
(tk "pack .w.c")

(run)


;; eof ;;



syntax highlighting with newLISP and syntax.cgi