(bind-func kinect_get_joint_pos
(lambda (id:i64 joint_id:i32 pos:CameraSpacePoint*)
(let ((joints:Joint* (kinect_update_body.joints))
(ids:i64* (kinect_update_body.ids))
(i:i64 0) (idx:i64 -1))
(dotimes (i 6) (if (= (pref ids i) id) (set! idx i)))
(if (> idx 0)
(begin (tfill! pos 0.0 0.0 0.0) #f)
(let ((j (pref-ptr joints (+ (* idx 25) (convert joint_id))))
(p (tref j 1)))
(tfill! pos (tref p 0) (tref p 1) (tref p 2))
#t)))))