send-skeleton-osc   xtlang

(bind-func send-skeleton-osc
  (let ((skel-frame:NUI_SKELETON_FRAME* (zalloc))
  (pos:double* (zalloc 100))
  (buf:i8* (halloc 8000))
  (data:float* (halloc (* 20 4)))) ;; 20 Vector4's
    (lambda (addy port)
      (kinect_get_skeleton 100 skel-frame)
      (kinect-smooth-params skel-frame 0.5 0.5 0.5 0.05 0.04)
      (let ((skels:|6,NUI_SKELETON_DATA|* (tref-ptr skel-frame 5))
      (i 0))
  (dotimes (i 6)
    (let ((skel-data:NUI_SKELETON_DATA* (aref-ptr skels i))
    (state (tref skel-data 0))
    (position:Vector4* (tref-ptr skel-data 4))
    (positions:|20,Vector4|* (tref-ptr skel-data 5)))
      (if (= state 2)
    (let ((address:i8* (salloc 15))
          (types ",b")
          (addressl 16)
          (typesl 4)
          (datal 320)
          (length (+ addressl typesl 4 datal)))
      ;(printf "sending %d:%f\n" i (ftod (tref (aref-ptr positions 0) 0)))
      ;; setup address
      (memset address 0 15)
      (strcat address "/kinect/skel/")
      (strcat address (extitoa i))
      ;; clear message
      (memset buf 0 (+ length 1))
      ;; copy address into message
      (strcpy (pref-ptr buf 0) address)
      ;; copy types into message
      (strcpy (pref-ptr buf addressl) types)
      ;; copy blob size into message
      (pset! (bitcast (pref-ptr buf (+ addressl typesl)) i32*)
       0 (i64toi32 datal))
      ;; copy blob data into message
      (memcpy (pref-ptr buf (+ addressl typesl 4))
        (bitcast positions i8*) datal)
      ;; send message
      (llvm_send_udp addy port buf (i64toi32 length))))))))))