src/magplot/magplot.scm

   1 ;; Copyright (C) 2009 Sean D'Epagnier <sean@depagnier.com>
   2 ;;
   3 ;; This Program is free software; you can redistribute it and/or
   4 ;; modify it under the terms of the GNU General Public
   5 ;; License as published by the Free Software Foundation; either
   6 ;; version 3 of the License, or (at your option) any later version.
   7 ;;
   8 ;; This program is distributed in the hope that it will be useful,
   9 ;; but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 ;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 ;; GNU General Public License for more details.
  12 ;;
  13 ;; You should have received a copy of the GNU General Public License
  14 ;; along with this program; if not, write to the Free Software
  15 ;; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  16 ;;
  17 ;; For more information on the GPL, please go to:
  18 ;; http://www.gnu.org/copyleft/gpl.html
  19
  20 ;; Need to install modules with chicken:
  21 ;;
  22 ;; chicken-setup opengl glut syntax-case pos args
  23
  24 (require-extension srfi-1 srfi-4 gl glut posix srfi-18 srfi-12 syntax-case pos)
  25
  26 ; some linear algebra functions
  27 (define (square x)
  28   (* x x))
  29 (define (cube x)
  30   (* x x x))
  31 (define (magnitude vec)
  32   (sqrt (apply + (map square vec))))
  33
  34 (define (normalize vec)
  35   (let ((mag (magnitude vec)))
  36     (map (lambda (x) (/ x mag)) vec)))
  37
  38 (define (dot . vecs)
  39   (apply + (apply map * vecs)))
  40
  41
  42 (define pi (* 2 (asin 1)))
  43 (define (deg2rad x) (* pi (/ x 180)))
  44
  45 (define horizdilution 1.5)
  46
  47 (define curpos '(0 0 0 0))
  48
  49 (define points '())
  50 (define triangles '())
  51 (define  (trianglegrid-addpoint x y z mag)
  52   (let ((p (list x y z mag)))
  53         (set! points (cons p points)))
  54   )
  55
  56 (define colorspace-min 1)
  57 (define colorspace-max 0)
  58 (define (recalc-colorspace)
  59   (for-each (lambda (p)
  60               (let ((c (fourth p)))
  61                 (if (< c colorspace-min)
  62                     (set! colorspace-min c))
  63                 (if (> c colorspace-max)
  64                     (set! colorspace-max c))))
  65             points))
  66
  67 (define (colorspace mag)
  68   (let ((diff (- colorspace-max colorspace-min)))
  69     (if (= diff 0) (set! diff .01))
  70     (let ((n (+ (/ (- mag (/ (+ colorspace-min colorspace-max) 2))
  71                    diff) .5)))
  72       (map (lambda (i) (- 1 (* 2 (abs (- i n))))) '(0 .5 1)))))
  73
  74 (define (make-gl-point p)
  75   (apply gl:Color3f (colorspace (cadddr p)))
  76   (gl:Vertex3f (car p) (cadr p) (caddr p)))
  77
  78 (define  (trianglegrid-draw)
  79 ;  (gl:Begin gl:POINTS)
  80 ;  (for-each make-gl-point points)
  81 ;  (gl:End)
  82   (recalc-colorspace)
  83   (for-each (lambda (point)
  84               (pos-draw point .1 (colorspace (cadddr point))))
  85             points)
  86
  87 )
  88
  89 (define point-pos car)
  90 (define point-color cadr)
  91 (define (new-point pos mag)
  92   (list pos mag))
  93
  94 (define (draw-point point)
  95   (let ((c (colorspace (point-color point))))
  96     (gl:Color3f c c c))
  97   (gl:Vertex3fv (point-pos point)))
  98
  99
 100 ; make syntax for various opengl features to make them nicer
 101 (define-syntax glBegin (syntax-rules ()
 102                          ((glBegin val body ...)
 103                           (begin (gl:Begin val) body ... (gl:End)))))
 104
 105 (define-syntax glLetMatrix (syntax-rules ()
 106                          ((glLetMatrix body ...)
 107                           (begin (gl:PushMatrix) body ... (gl:PopMatrix)))))
 108
 109 (define-syntax glLetEnable (syntax-rules ()
 110                              ((glLetEnable (val ...) body ...)
 111                               (begin (begin (gl:Enable val) ...)
 112                                      (begin body ...)
 113                                      (begin (gl:Disable val) ...)))))
 114
 115 (define (draw-triangles triangles)
 116   (let ((x (apply min points))
 117         (y (apply max points)))
 118
 119   (glBegin gl:TRIANGLES
 120            (for-each (lambda (triangle)
 121                        (draw-point (car point))
 122                        (draw-point (cadr point))
 123                        (draw-point (caddr point)))
 124                      triangles))))
 125
 126 (define (RotateAfter ang x y z)
 127   (let ((m (f32vector)))
 128     (gl:GetFloatv gl:MODELVIEW_MATRIX m)
 129     (gl:LoadIdentity)
 130     (gl:Rotatef ang x y z)
 131     (gl:MultMatrixf m)))
 132
 133 (define (reshape w h)
 134   (gl:Viewport 0 0 w h)
 135   (gl:MatrixMode gl:PROJECTION)
 136   (gl:LoadIdentity)
 137   (let ((ar (/ w h)))
 138     (gl:Frustum (* -.1 ar) (* .1 ar) -.1 .1 .1 100))
 139   (gl:MatrixMode gl:MODELVIEW)
 140   (gl:LoadIdentity)
 141   (gl:Translatef 0.0 0.0 -10.0))
 142
 143 ; render an alpha blended circle of the current gps location
 144 (define (pos-draw pos radius color)
 145   (gl:BlendFunc gl:SRC_ALPHA gl:ONE_MINUS_SRC_ALPHA)
 146   (glLetEnable
 147    (gl:BLEND)
 148    (glLetMatrix
 149     (gl:Translatef (car pos) (cadr pos) (caddr pos))
 150     (glBegin gl:TRIANGLE_FAN
 151              (gl:Color4f .5 .5 .5 1)
 152              (gl:Vertex3f 0 0 0)
 153              (gl:Color4f (first color) (second color) (third color) 0)
 154              (do ((x 0 (+ x (/ pi 10))))
 155                  ((> x (+ (* 2 pi) .1)))
 156                (gl:Vertex3f (* radius (cos x)) (* radius (sin x)) 0))))))
 157
 158 ; called at the start of each frame to display
 159 (define (gl-display)
 160   (gl:Clear gl:COLOR_BUFFER_BIT)
 161
 162   (glLetMatrix
 163    (gl:Translatef 0 0 -1)
 164    (trianglegrid-draw)
 165    (pos-draw curpos horizdilution '(1 0 0)))
 166
 167   (glut:SwapBuffers))
 168
 169 (define (none-false l)
 170   (fold (lambda (el acc) (and el acc)) #t l))
 171
 172 (define magmag 0)
 173 (define (parse-mag-line line)
 174   (let ((vals (string-split line)))
 175     (if (and (>= (length vals) 3)
 176              (equal? (car vals) "mag:"))
 177         (let ((magv (map string->number (take (cdr vals) 3))))
 178           (if (none-false magv)
 179               (set! magmag (magnitude magv)))))))
 180
 181 ; convert lat lon and alt to x, y, z coordinates used for plotting
 182 (define convert-position
 183   (let ((flat 100)
 184         (flon 100)
 185         (falt 100))
 186     (lambda (lat lon alt)
 187       (cond ((= flat 100)
 188              (set! flat lat)
 189              (set! flon lon)
 190              (set! falt alt)))
 191       (let ((earthradius 6360000))
 192         (list
 193          (* earthradius (sin (deg2rad (- lon flon))) (cos (deg2rad lat)))
 194          (* earthradius (sin (deg2rad (- lat flat))))
 195          (/ (- alt falt) 10) ; scale down alt a lot for now
 196             )))))
 197
 198 (define (parse-gps-line line)
 199   (let ((vals (string-split line ",")))
 200     (cond ((null? vals) '())
 201           ((and (> (length vals) 9)
 202                 (equal? (car vals) "$GPGGA"))
 203            (let ((time (string->number (list-ref vals 1)))
 204                  (north (string->number (list-ref vals 2)))
 205                  (ns (list-ref vals 3))
 206                  (east (string->number (list-ref vals 4)))
 207                  (ew (list-ref vals 5))
 208                  (fix (string->number (list-ref vals 6)))
 209                  (satcount (string->number (list-ref vals 7)))
 210                  (hd (string->number (list-ref vals 8)))
 211                  (alt (string->number (list-ref vals 9))))
 212              (if (= fix 0)
 213                  (print "no gps fix, sats: " satcount)
 214                  (let ()
 215                    (define (convert-deg val)
 216                      (+ (floor (/ val 100))
 217                         (/ (remainder val 100) 60)))
 218                    (if (< fix 1) ; hacked to turn on without DGPS
 219                        (print "warning, only normal GPS not DGPS fix, discarding")
 220                        (begin
 221                          (if (equal? "S" ns)
 222                              (set! north (- north)))
 223                          (if (equal? "W" ew)
 224                              (set! east (- east)))
 225                          (if verbose
 226                              (print north "N " east "E " alt "M " hd " HD "
 227                                     (if (= 1 fix) " GPS fix " "DGPS fix ") satcount " sats"))
 228                          (set! curpos (append
 229                                        (convert-position
 230                                         (convert-deg north)
 231                                         (convert-deg east)
 232                                         alt)
 233                                        (list magmag)))
 234                          (set! horizdilution hd)
 235                          (reportdata curpos))))))))))
 236
 237 (define (make-line-reader)
 238   (let ((cur ""))
 239     (lambda (port)
 240       (if (char-ready? port)
 241           (let loop ()
 242             (let ((c (read-char port)))
 243               (cond ((eof-object? c) #f)
 244                     ((equal? c #\newline)
 245                      (let ((ret (string-copy cur)))
 246                        (set! cur "")
 247                        ret))
 248                     (else (set! cur (string-append cur (string c))) (loop)))))
 249           #f))))
 250
 251 (define idle
 252   (let ((mag-line (make-line-reader))
 253         (gps-line (make-line-reader)))
 254     (lambda ()
 255       (if mag-device
 256           (let each-mag-line ()
 257             (let ((line (mag-line mag-device)))
 258               (cond (line (parse-mag-line line)
 259                           (each-mag-line))))))
 260       (if gps-device
 261           (let each-gps-line ()
 262             (let ((line (gps-line gps-device)))
 263               (cond (line (parse-gps-line line)
 264                           (each-gps-line))))))
 265       (thread-sleep! .001)
 266       )))
 267
 268 (require-extension srfi-69)
 269 (define attributes (make-hash-table))
 270 (define (attr-set! name val)
 271   (hash-table-set! attributes name val))
 272 (define (attr-get name)
 273   (hash-table-ref attributes name))
 274 (attr-set! "linemode" #f)
 275 (attr-set! "rotatespeed" 5)
 276 (attr-set! "translatespeed" 1)
 277
 278 (define (glut-HasModifiers . modifiers)
 279   (let ((modifiers (apply bitwise-ior modifiers)))
 280     (= modifiers (bitwise-and modifiers (glut:GetModifiers)))))
 281
 282 (define (keyboard key x y)
 283   (case key
 284     ((#\esc #\q) (exit))
 285     ((#\w)
 286      (attr-set! "linemode" (not (attr-get "linemode")))
 287      (gl:PolygonMode gl:FRONT_AND_BACK
 288                      (if (attr-get "linemode")
 289                          gl:LINE gl:FILL)))
 290     ((#\f) (glut:FullScreen)))
 291   (glut:PostRedisplay))
 292
 293 (define (special key x y)
 294   (if (glut-HasModifiers glut:ACTIVE_SHIFT)
 295       (let ((rs (attr-get "rotatespeed")))
 296         (cond
 297           ((= key glut:KEY_LEFT) (RotateAfter rs 0 1 0))
 298           ((= key glut:KEY_RIGHT) (RotateAfter rs 0 -1 0))
 299           ((= key glut:KEY_UP) (RotateAfter rs 1 0 0))
 300           ((= key glut:KEY_DOWN) (RotateAfter rs -1 0 0))
 301           ((= key glut:KEY_PAGE_UP) (RotateAfter rs 0 0 1))
 302           ((= key glut:KEY_PAGE_DOWN) (RotateAfter rs 0 0 -1))))
 303       (let ((ts (attr-get "translatespeed")))
 304         (cond
 305           ((= key glut:KEY_LEFT) (gl:Translatef ts 0 0))
 306           ((= key glut:KEY_RIGHT) (gl:Translatef (- ts) 0 0))
 307           ((= key glut:KEY_UP) (gl:Translatef 0 (- ts) 0))
 308           ((= key glut:KEY_DOWN) (gl:Translatef 0 ts 0))
 309           ((= key glut:KEY_PAGE_UP) (gl:Translatef 0 0 (- ts)))
 310           ((= key glut:KEY_PAGE_DOWN) (gl:Translatef 0 0 ts))
 311         )))
 312   (glut:PostRedisplay))
 313
 314 (glut:InitDisplayMode (+ glut:DOUBLE glut:RGB glut:ALPHA))
 315 (glut:CreateWindow "magplot")
 316 (glut:ReshapeFunc reshape)
 317 (glut:DisplayFunc gl-display)
 318 (glut:IdleFunc idle)
 319 (glut:KeyboardFunc keyboard)
 320 (glut:SpecialFunc special)
 321
 322 (gl:ClearColor 0.0 0.0 0.0 0)
 323
 324 (use args)
 325
 326 (define (usage)
 327   (print "Usage: " (car (argv)) " [options...] [files...]")
 328   (newline)
 329   (print (args:usage opts))
 330   (print "Report bugs to geckosenator at gmail.")
 331   (exit 1))
 332
 333 (define output-file #f)
 334 (define noshow #f)
 335 (define verbose #f)
 336 (define gps-device #f)
 337 (define mag-device #f)
 338
 339 (define (reportdata pos)
 340   (let ((x (car pos))
 341         (y (cadr pos))
 342         (z (caddr pos))
 343         (mag (cadddr pos)))
 344     (if output-file
 345         (with-output-to-port output-file
 346           (lambda () (print x " " y " " z " " mag))))
 347     (trianglegrid-addpoint x y z mag)
 348     (if (not noshow)
 349         (glut:PostRedisplay))))
 350
 351 (define (load-data)
 352   (let ((x (read)))
 353     (let ((y (read)))
 354       (let ((z (read)))
 355         (let ((mag (read)))
 356           (cond ((not (eof-object? mag))
 357                  (reportdata (list x y z mag))
 358                  (load-data))))))))
 359
 360 (define (handle-open-file filename thunk)
 361   (thunk))
 362 ;  (with-exception-handler
 363 ;   (lambda _ (print "failed to open file: " filename) (exit))
 364 ;   thunk))
 365
 366 (define opts
 367  (list
 368   (args:make-option (h help) #:none "Display this text" (usage))
 369   (args:make-option (f file) (required: "FILENAME") "read input from a file"
 370                     (handle-open-file arg
 371                                       (lambda ()
 372                                         (with-input-from-port
 373                                             (if (equal? "-" arg)
 374                                                 (current-input-port)
 375                                                 (open-input-file arg))
 376                                               load-data))))
 377   (args:make-option (o output) (required: "FILENAME") "write output to a file"
 378                     (if (equal? "-" arg)
 379                         (set! output-file (current-output-port))
 380                         (handle-open-file arg
 381                                           (lambda ()
 382                                             (set! output-file (open-output-file arg))))))
 383   (args:make-option (n noshow) #:node  "Don't display anything"
 384                     (set! noshow #t))
 385   (args:make-option (v verbose) #:node  "Don't display anything"
 386                     (set! verbose #t))
 387   (args:make-option (g gps)    (required: "DEVICE") "device for gps data"
 388                     ; set baud to 19200
 389                     (system (string-append "stty -F " arg " 19200"))
 390                     (handle-open-file arg
 391                                       (lambda ()
 392                                         (set! gps-device (open-input-file arg)))))
 393   (args:make-option (m mag) (required: "DEVICE") "device for magnetometer data"
 394                     (handle-open-file arg
 395                                       (lambda ()
 396                                         (set! mag-device (open-input-file arg)))))
 397   ))
 398
 399 (args:parse (command-line-arguments) opts)
 400
 401 (if noshow
 402     (let loop () (idle) (loop))
 403     (glut:MainLoop))