Permalink
Fetching contributors…
Cannot retrieve contributors at this time
143 lines (124 sloc) 5.28 KB
;;; kinect.xtm -- Kinect for Windows
;; Author: Andrew Sorensen
;; Keywords: extempore
;; Required dylibs: kinectlib
;;; Commentary:
;;
;;; Code:
(bind-dylib kinectlib
(cond ((string=? (sys:platform) "OSX")
(begin (println "Kinect Not Supported on Linux") #f))
((string=? (sys:platform) "Linux")
(begin (println "Kinect Not Supported on Linux") #f))
((string=? (sys:platform) "Windows")
"kinectlib.dll")))
(bind-type Vector4 <float,float,float,float>)
(bind-type NUI_SKELETON_DATA <i32,i32,i32,i32,Vector4,|20,Vector4|,|20,i32|,i32>)
(bind-type NUI_SKELETON_FRAME <i64,i32,i32,Vector4,Vector4,|6,NUI_SKELETON_DATA|>)
(bind-type NUI_TRANSFORM_SMOOTH_PARAMETERS <float,float,float,float,float>)
(bind-val NUI_INITIALIZE_FLAG_USES_AUDIO i32 268435456)
(bind-val NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX i32 1)
(bind-val NUI_INITIALIZE_FLAG_USES_COLOR i32 2)
(bind-val NUI_INITIALIZE_FLAG_USES_SKELETON i32 8)
(bind-val NUI_INITIALIZE_FLAG_USES_DEPTH i32 32)
(bind-val NUI_INITIALIZE_DEFAULT_HARDWARE_THREAD i32 4294967295)
(bind-val NUI_SKELETON_POSITION_HIP_CENTER i32 0)
(bind-val NUI_SKELETON_POSITION_SPINE i32 1)
(bind-val NUI_SKELETON_POSITION_SHOULDER_CENTER i32 2)
(bind-val NUI_SKELETON_POSITION_HEAD i32 3)
(bind-val NUI_SKELETON_POSITION_SHOULDER_LEFT i32 4)
(bind-val NUI_SKELETON_POSITION_ELBOW_LEFT i32 5)
(bind-val NUI_SKELETON_POSITION_WRIST_LEFT i32 6)
(bind-val NUI_SKELETON_POSITION_HAND_LEFT i32 7)
(bind-val NUI_SKELETON_POSITION_SHOULDER_RIGHT i32 8)
(bind-val NUI_SKELETON_POSITION_ELBOW_RIGHT i32 9)
(bind-val NUI_SKELETON_POSITION_WRIST_RIGHT i32 10)
(bind-val NUI_SKELETON_POSITION_HAND_RIGHT i32 11)
(bind-val NUI_SKELETON_POSITION_HIP_LEFT i32 12)
(bind-val NUI_SKELETON_POSITION_KNEE_LEFT i32 13)
(bind-val NUI_SKELETON_POSITION_ANKLE_LEFT i32 14)
(bind-val NUI_SKELETON_POSITION_FOOT_LEFT i32 15)
(bind-val NUI_SKELETON_POSITION_HIP_RIGHT i32 16)
(bind-val NUI_SKELETON_POSITION_KNEE_RIGHT i32 17)
(bind-val NUI_SKELETON_POSITION_ANKLE_RIGHT i32 18)
(bind-val NUI_SKELETON_POSITION_FOOT_RIGHT i32 19)
(bind-val NUI_SKELETON_POSITION_COUNT i32 20)
(bind-lib kinectlib kinect_start [i32,i32]*)
(bind-lib kinectlib kinect_get_skeleton [i32,i64,NUI_SKELETON_FRAME*]*)
(bind-lib kinectlib kinect_shutdown [void]*)
(bind-lib kinectlib kinect_smooth [i32,NUI_SKELETON_FRAME*,NUI_TRANSFORM_SMOOTH_PARAMETERS*]*)
(bind-func kinect-start
(lambda (v:i32)
(printf "kinect started: %d\n" (kinect_start NUI_INITIALIZE_FLAG_USES_SKELETON))))
(bind-func kinect-shutdown
(lambda ()
(kinect_shutdown)))
(bind-func kinect-smooth-params
(let ((smooth_params:NUI_TRANSFORM_SMOOTH_PARAMETERS* (zalloc)))
(lambda (skeleton smoothing correction prediction jitter_radius max_deviation_radius)
(tfill! smooth_params smoothing correction prediction jitter_radius max_deviation_radius)
(kinect_smooth skeleton smooth_params))))
(bind-func kinect-smooth
(let ((smooth_params:NUI_TRANSFORM_SMOOTH_PARAMETERS* (zalloc)))
(tfill! smooth_params (dtof 0.5) (dtof 0.5) (dtof 0.5) (dtof 0.05) (dtof 0.04))
(lambda (skeleton)
(kinect_smooth skeleton smooth_params))))
(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))))))))))
(bind-val _skeletons_ |6,|20,Vector4||* (* 6 (* 20 4 4)))
(bind-val _skeletons-alive_ |6,i1|)
(bind-func receive-skel-osc
(lambda (address:i8* types:i8* args:i8* size:i32)
(if (= (strncmp address "/kinect/skel" 12) 0)
(let ((skel-num (- (i8toi64 (pref address 13)) 48))
(positions (pref-ptr args 4))
(posarg (bitcast positions |20,Vector4|*))
(i 0) (k 0)
(skel (aref-ptr _skeletons_ skel-num)))
;(printf "receiving %d:%f\n" skel-num (ftod (tref (aref-ptr posarg 0) 0)))
(dotimes (i 20)
(let ((v4p (aref-ptr posarg i)))
(tfill! (aref-ptr skel i) (tref v4p 0) (tref v4p 1) (tref v4p 2) (tref v4p 3))))
;(memcpy skel positions (* 20 4 4))
(aset! _skeletons-alive_ skel-num 1)
1)
1)))