1.1 --- a/org/test-creature.org	Wed Feb 01 02:27:18 2012 -0700
     1.2 +++ b/org/test-creature.org	Wed Feb 01 02:44:07 2012 -0700
     1.3 @@ -1058,16 +1058,20 @@
     1.4  ;; these are the functions that provide world i/o, chinese-room style
     1.5  
     1.6  
     1.7 +(defn creature-joints 
     1.8 +  "Return the children of the creature's \"joints\" node."
     1.9 +  [#^Node creature]
    1.10 +  (if-let [joint-node (.getChild creature "joints")]
    1.11 +    (seq (.getChildren joint-node))
    1.12 +    (do (println-repl "could not find JOINTS node") [])))
    1.13 +
    1.14  
    1.15  (defn blender-creature
    1.16    "Return a creature with all joints in place."
    1.17    [blender-path]
    1.18    (let [model (load-blender-model blender-path)
    1.19 -        joints
    1.20 -        (if-let [joint-node (.getChild model "joints")]
    1.21 -          (seq (.getChildren joint-node))
    1.22 -          (do (println-repl "could not find joints node") []))]
    1.23 -  (assemble-creature model joints)))
    1.24 +        joints (creature-joints model)]
    1.25 +    (assemble-creature model joints)))
    1.26  
    1.27  (defn gray-scale [num]
    1.28    (+ num
    1.29 @@ -1167,7 +1171,49 @@
    1.30            [(rad->deg yaw)
    1.31             (rad->deg pitch)
    1.32             (rad->deg roll)])
    1.33 -       sensor-data)))))
    1.34 +        sensor-data)))))
    1.35 +
    1.36 +
    1.37 +(defn draw-sprite [image sprite x y color ]
    1.38 +  (dorun
    1.39 +   (for [[u v] sprite]
    1.40 +     (.setRGB image (+ u x) (+ v y) color))))
    1.41 +
    1.42 +(defn view-angle
    1.43 +  "create a debug view of an angle"
    1.44 +  [color]
    1.45 +  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
    1.46 +        previous (atom [25 25])
    1.47 +        sprite [[0 0] [0 1]
    1.48 +                [0 -1] [-1 0] [1 0]]] 
    1.49 +    (fn [angle]
    1.50 +      (let [angle (float angle)]
    1.51 +        (let [position
    1.52 +              [(+ 25 (int (* 20 (Math/cos angle))))
    1.53 +               (+ 25 (int (* 20(Math/sin angle))))]]
    1.54 +          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
    1.55 +          (draw-sprite image sprite (position 0) (position 1) color)
    1.56 +          (reset! previous position))
    1.57 +        image))))
    1.58 +
    1.59 +(defn proprioception-debug-window
    1.60 +  []
    1.61 +  (let [yaw (view-angle 0xFF0000)
    1.62 +        roll (view-angle 0x00FF00)
    1.63 +        pitch (view-angle 0xFFFFFF)
    1.64 +        v-yaw (view-image)
    1.65 +        v-roll (view-image)
    1.66 +        v-pitch (view-image)
    1.67 +        ]
    1.68 +    (fn [prop-data]
    1.69 +      (dorun
    1.70 +       (map
    1.71 +        (fn [[y r p]]
    1.72 +          (v-yaw (yaw y))
    1.73 +          (v-roll (roll r))
    1.74 +          (v-pitch (pitch p)))
    1.75 +        prop-data)))))
    1.76 +
    1.77  
    1.78  
    1.79