/examples/springies/source.scm
Scheme | 441 lines | 258 code | 138 blank | 45 comment | 0 complexity | f23320325339aee2e8ae43dd4074a3b6 MD5 | raw file
Possible License(s): BSD-3-Clause
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