/examples/springies/source.scm

  1
  2;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  3
  4(define (vector-nth i) (cut vector-ref <> i))
  5
  6;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  7
  8(define (scalar-projection a b)
  9  (/ (dot a b)
 10     (norm b)))
 11
 12(define (vector-projection a b)
 13  (pt*n (normalize b)
 14        (scalar-projection a b)))
 15
 16;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 17
 18(define (pt-sum-4 a b c d)
 19  (pt+ (pt+ (pt+ a b) c) d))
 20
 21;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 22
 23(define *nodes* #f)
 24(define *springs* #f)
 25
 26(define *time-slice* #f)
 27
 28(define *gravity* #t)
 29
 30;; (define *world-width*  #f)
 31;; (define *world-height* #f)
 32
 33(define *world-width*  500)
 34(define *world-height* 500)
 35
 36;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 37
 38(define (node-id id)
 39  (list-ref *nodes* (- id 1)))
 40
 41;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 42
 43(define (node pos vel mass elas)
 44
 45  (let ((acc   (pt 0.0 0.0))
 46        
 47        (force (pt 0.0 0.0))
 48
 49        (cur-pos #f)
 50        (cur-vel #f)
 51        (pos-k1  #f)
 52        (vel-k1  #f)
 53        (pos-k2  #f)
 54        (vel-k2  #f)
 55        (pos-k3  #f)
 56        (vel-k3  #f)
 57        (pos-k4  #f)
 58        (vel-k4  #f))
 59
 60    (let ((apply-force
 61           (lambda (v)
 62             (set! force (pt+ force v))))
 63
 64          (reset-force
 65           (lambda ()
 66             (set! force (pt 0 0))))
 67
 68          ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 69
 70          (new-acc (lambda () (pt/n force mass)))
 71
 72          (new-vel (lambda () (pt+ vel (pt*n acc *time-slice*))))
 73          (new-pos (lambda () (pt+ pos (pt*n vel *time-slice*))))
 74
 75          ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 76
 77          (k1-step
 78           (lambda ()
 79             (set! cur-pos pos)
 80             (set! cur-vel vel)
 81
 82             (set! pos-k1 (pt*n vel *time-slice*))
 83             (set! vel-k1 (pt*n acc *time-slice*))
 84
 85             (set! pos (pt+ cur-pos (pt/n pos-k1 2.0)))
 86             (set! vel (pt+ cur-vel (pt/n vel-k1 2.0)))))
 87
 88          (k2-step
 89           (lambda ()
 90             (set! pos-k2 (pt*n vel *time-slice*))
 91             (set! vel-k2 (pt*n acc *time-slice*))
 92
 93             (set! pos (pt+ cur-pos (pt/n pos-k2 2.0)))
 94             (set! vel (pt+ cur-vel (pt/n vel-k2 2.0)))))
 95
 96          (k3-step
 97           (lambda ()
 98             (set! pos-k3 (pt*n vel *time-slice*))
 99             (set! vel-k3 (pt*n acc *time-slice*))
100
101             (set! pos (pt+ cur-pos pos-k3))
102             (set! vel (pt+ cur-vel vel-k3))))
103
104          (k4-step
105           (lambda ()
106             (set! pos-k4 (pt*n vel *time-slice*))
107             (set! vel-k4 (pt*n acc *time-slice*))))
108
109          (find-next-position
110
111           (let ((handle-bounce
112
113                  (let ((below? (lambda () (< (y pos) 0)))
114
115                        (above? (lambda () (>= (y pos) *world-height*)))
116
117                        (beyond-left? (lambda () (< (x pos) 0)))
118
119                        (beyond-right? (lambda () (>= (x pos) *world-width*)))
120
121                        (bounce-top
122                         (lambda ()
123                           (y! pos (- *world-height* 1.0))
124                           (y! vel (- (* (y vel) elas)))))
125
126                        (bounce-bottom
127                         (lambda ()
128                           (y! pos 0.0)
129                           (y! vel (- (* (y vel) elas)))))
130
131                        (bounce-left
132                         (lambda ()
133                           (x! pos 0.0)
134                           (x! vel (- (* (x vel) elas)))))
135
136                        (bounce-right
137                         (lambda ()
138                           (x! pos (- *world-width* 1.0))
139                           (x! vel (- (* (x vel) elas))))))
140                  
141                    (lambda ()
142                      (cond ((above?)        (bounce-top))
143                            ((below?)        (bounce-bottom))
144                            ((beyond-left?)  (bounce-left))
145                            ((beyond-right?) (bounce-right))
146                            (else 'ok))))))
147               
148               (lambda ()
149
150                 (set! pos (pt+ cur-pos
151                                (pt/n (pt-sum-4 (pt/n pos-k1 2.0)
152                                                pos-k2
153                                                pos-k3
154                                                (pt/n pos-k4 2.0))
155                                      3.0)))
156
157                 (set! vel (pt+ cur-vel
158                                (pt/n (pt-sum-4 (pt/n vel-k1 2.0)
159                                                vel-k2
160                                                vel-k3
161                                                (pt/n vel-k4 2.0))
162                                      3.0)))
163
164                 (handle-bounce)))))
165
166      (let ((update-acceleration
167             (lambda ()
168               (set! acc (new-acc))
169               (reset-force))))
170
171      (vector 'node
172              (lambda () pos)
173              (lambda () vel)
174              apply-force
175              k1-step
176              k2-step
177              k3-step
178              k4-step
179              find-next-position
180              update-acceleration
181              (lambda (new) (set! vel new)))
182      ))))
183
184(define (pos node) ((vector-ref node 1)))
185(define (vel node) ((vector-ref node 2)))
186
187(define (apply-force node v) ((vector-ref node 3) v))
188
189(define (k1-step node) ((vector-ref node 4)))
190(define (k2-step node) ((vector-ref node 5)))
191(define (k3-step node) ((vector-ref node 6)))
192(define (k4-step node) ((vector-ref node 7)))
193
194(define (find-next-position node) ((vector-ref node 8)))
195
196(define (update-acceleration node) ((vector-ref node 9)))
197
198(define (vel! node new) ((vector-ref node 10) new))
199
200;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
201
202(define (apply-gravity node)
203  (apply-force node (pt 0 -9.8)))
204
205(define (do-gravity)
206  (if *gravity*
207      (for-each apply-gravity *nodes*)))
208
209;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
210;; spring
211;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
212
213(define (spring rest-length k damp node-a node-b)
214  (vector 'spring rest-length k damp node-a node-b))
215
216(define rest-length (vector-nth 1))
217(define k           (vector-nth 2))
218(define damp        (vector-nth 3))
219(define node-a      (vector-nth 4))
220(define node-b      (vector-nth 5))
221
222(define (spring-length spr)
223  (norm (pt- (pos (node-b spr))
224             (pos (node-a spr)))))
225
226(define (stretch-length spr)
227  (- (spring-length spr)
228     (rest-length   spr)))
229
230(define (dir spr)
231  (normalize (pt- (pos (node-b spr))
232                  (pos (node-a spr)))))
233
234;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
235;; Hooke
236;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
237;; 
238;; F = -kx
239;; 
240;; k :: spring constant
241;; x :: distance stretched beyond rest length
242;; 
243;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
244
245(define hooke-force-mag (bi k stretch-length *)) ;; spring -- mag
246
247(define hooke-force (bi dir hooke-force-mag pt*n)) ;; spring -- force
248
249(define (act-on-nodes-hooke spr)
250
251  (let ((F (hooke-force spr)))
252
253    (apply-force (node-a spr)         F)
254    (apply-force (node-b spr) (pt-neg F))))
255
256;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
257;; damping
258;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
259;; 
260;; F = -bv
261;; 
262;; b :: Damping constant
263;; v :: Velocity
264;;
265;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
266
267(define relative-velocity-a (bi (uni node-a vel) (uni node-b vel) pt-))
268
269(define unit-vec-b->a (bi (uni node-a pos) (uni node-b pos) pt-))
270
271(define relative-velocity-along-spring-a ;; spring -- vel
272  (bi relative-velocity-a unit-vec-b->a vector-projection))
273
274(define damping-force-a
275  (bi relative-velocity-along-spring-a damp (uni2 pt*n pt-neg)))
276
277;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
278
279(define relative-velocity-b (bi (uni node-b vel) (uni node-a vel) pt-))
280
281(define unit-vec-a->b (bi (uni node-b pos) (uni node-a pos) pt-))
282
283(define relative-velocity-along-spring-b ;; spring -- vel
284  (bi relative-velocity-b unit-vec-a->b vector-projection))
285
286(define damping-force-b ;; spring -- vec
287  (bi relative-velocity-along-spring-b damp (uni2 pt*n pt-neg)))
288
289;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
290
291(define (act-on-nodes-damping spr)
292  (apply-force (node-a spr) (damping-force-a spr))
293  (apply-force (node-b spr) (damping-force-b spr)))
294
295;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
296
297(define (act-on-nodes spr)
298  (act-on-nodes-hooke   spr)
299  (act-on-nodes-damping spr))
300
301(define (loop-over-springs)
302  (for-each act-on-nodes *springs*))
303
304;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
305
306(define (update-nodes-acceleration)
307  (for-each update-acceleration *nodes*))
308
309(define (accumulate-acceleration)
310  (do-gravity)
311  (loop-over-springs)
312  (update-nodes-acceleration))
313
314;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
315
316(define (iterate-system-runge-kutta)
317
318  (accumulate-acceleration) (for-each k1-step *nodes*)
319  (accumulate-acceleration) (for-each k2-step *nodes*)
320  (accumulate-acceleration) (for-each k3-step *nodes*)
321  (accumulate-acceleration) (for-each k4-step *nodes*)
322
323  (for-each find-next-position *nodes*))
324
325(define iterate-system iterate-system-runge-kutta)
326
327;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
328
329(define (mass id x y x-vel y-vel mass elas)
330  (set! *nodes*
331        (append *nodes*
332                (list
333                 (node (pt x y) (pt x-vel y-vel) mass elas)))))
334
335(define (spng id id-a id-b k damp rest-length)
336  (set! *springs*
337        (append *springs*
338                (list
339                 (spring rest-length k damp (node-id id-a) (node-id id-b))))))
340
341;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
342
343(define (pt-gl-vertex p) (glVertex2d (x p) (y p)))
344
345(define (draw-node node)
346
347  (let ((pos (pos node)))
348
349    (glBegin GL_LINE_LOOP)
350
351    (pt-gl-vertex (pt+ pos '#(pt -5 -5)))
352    (pt-gl-vertex (pt+ pos '#(pt  5 -5)))
353    (pt-gl-vertex (pt+ pos '#(pt  5  5)))
354    (pt-gl-vertex (pt+ pos '#(pt -5  5)))
355
356    (glEnd)))
357
358;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
359
360(define (draw-spring spring)
361
362  (glBegin GL_LINES)
363
364  (pt-gl-vertex (pos (node-a spring)))
365  (pt-gl-vertex (pos (node-b spring)))
366
367  (glEnd))
368
369;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
370
371(define (draw-nodes)   (for-each draw-node   *nodes*))
372(define (draw-springs) (for-each draw-spring *springs*))
373
374(define (display-system)
375
376  (glClearColor 0.0 0.0 0.0 1.0)
377
378  (glClear GL_COLOR_BUFFER_BIT)
379
380  (draw-nodes)
381
382  (draw-springs))
383
384;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
385
386(define (run-springies)
387
388  (glutInitDisplayMode GLUT_DOUBLE)
389
390  (glutInit (vector 0) (vector ""))
391
392  (glutInitWindowSize 500 500)
393
394  (glutCreateWindow "Springies")
395
396  (glutReshapeFunc
397   (lambda (w h)
398
399     (set! *world-width*  w)
400     (set! *world-height* h)
401
402     (glEnable GL_BLEND)
403     (glBlendFunc GL_SRC_ALPHA GL_ONE_MINUS_SRC_ALPHA)
404     (glViewport 0 0 w h)
405
406     (glMatrixMode GL_PROJECTION)
407     (glLoadIdentity)
408
409     (glOrtho 0.0 (- *world-width* 1.0) 0.0 (- *world-height* 1.0) -1.0 1.0)))
410
411  (glutDisplayFunc
412   (lambda ()
413     (glMatrixMode GL_MODELVIEW)
414     (glLoadIdentity)
415     (display-system)
416     (glutSwapBuffers)))
417
418  (glutIdleFunc
419   (lambda ()
420     (iterate-system)
421     (glutPostRedisplay)))
422
423  (glutKeyboardFunc
424   (lambda (key x y)
425
426     (let ((key (if (char? key) key (integer->char key))))
427
428       (case key
429
430         ((#\2)
431          (set! *time-slice* (- *time-slice* 0.01))
432          (print "*time-slice* is now " *time-slice* "\n"))
433
434         ((#\3)
435          (set! *time-slice* (+ *time-slice* 0.01))
436          (print "*time-slice* is now " *time-slice* "\n"))))))
437
438  (glutMainLoop))
439
440;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
441