1.1 --- a/org/body.org	Mon Jan 30 05:47:51 2012 -0700
     1.2 +++ b/org/body.org	Mon Jan 30 07:21:46 2012 -0700
     1.3 @@ -56,9 +56,12 @@
     1.4           (.subtract 
     1.5            (.localToWorld object-a (.getPivotA joint) nil)
     1.6            (.getWorldTranslation object-a)))
     1.7 +
     1.8 +        ;; this is probably wrong!
     1.9          rotate-a
    1.10           (doto (Matrix3f.)
    1.11             (.fromStartEndVectors arm-a Vector3f/UNIT_X))
    1.12 +        
    1.13          arm-b
    1.14          (.mult
    1.15           rotate-a 
    1.16 @@ -85,6 +88,30 @@
    1.17      ;;(println-repl (.getName object-a) (.getName object-b))
    1.18      [pitch yaw roll]))
    1.19  
    1.20 +
    1.21 +(defn joint-proprioception
    1.22 +  [joint]
    1.23 +  (let [object-a (.getUserObject (.getBodyA joint))
    1.24 +        object-b (.getUserObject (.getBodyB joint))
    1.25 +
    1.26 +        arm-a
    1.27 +        (.normalize
    1.28 +         (.subtract 
    1.29 +          (.localToWorld object-a (.getPivotA joint) nil)
    1.30 +          (.getWorldTranslation object-a)))
    1.31 +        arm-b
    1.32 +        (.normalize
    1.33 +         (.subtract 
    1.34 +          (.localToWorld object-b (.getPivotB joint) nil)
    1.35 +          (.getWorldTranslation object-b)))
    1.36 +        
    1.37 +        
    1.38 +        ]
    1.39 +    
    1.40 +        
    1.41 +   
    1.42 +
    1.43 +
    1.44  (defn proprioception
    1.45    "Create a function that provides proprioceptive information about an
    1.46    entire body."
    1.47 @@ -262,21 +289,59 @@
    1.48         (set-gravity world Vector3f/ZERO)
    1.49         )
    1.50       no-op)))
    1.51 -     
    1.52 +(import java.awt.image.BufferedImage)
    1.53 +
    1.54 +(defn draw-sprite [image sprite x y color ]
    1.55 +  (dorun
    1.56 +   (for [[u v] sprite]
    1.57 +     (.setRGB image (+ u x) (+ v y) color))))
    1.58 +
    1.59 +(defn view-angle
    1.60 +  "create a debug biew of an angle"
    1.61 +  [color]
    1.62 +  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
    1.63 +        previous (atom [25 25])
    1.64 +        sprite [[0 0] [0 1]
    1.65 +                [0 -1] [-1 0] [1 0]]] 
    1.66 +    (fn [angle]
    1.67 +      (let [angle (float angle)]
    1.68 +        (let [position
    1.69 +              [(+ 25 (int (* 20 (Math/cos angle))))
    1.70 +               (+ 25 (int (* 20(Math/sin angle))))]]
    1.71 +          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
    1.72 +          (draw-sprite image sprite (position 0) (position 1) color)
    1.73 +          (reset! previous position))
    1.74 +        image))))
    1.75 +
    1.76  (defn proprioception-debug-window
    1.77    []
    1.78 -  (let [vi (view-image)]
    1.79 +  (let [yaw (view-angle 0xFF0000)
    1.80 +        pitch (view-angle 0x00FF00)
    1.81 +        roll (view-angle 0xFFFFFF)
    1.82 +        v-yaw (view-image)
    1.83 +        v-pitch (view-image)
    1.84 +        v-roll (view-image)
    1.85 +        ]
    1.86      (fn [prop-data]
    1.87        (dorun
    1.88         (map
    1.89 -       (comp 
    1.90 -        println-repl
    1.91          (fn [[p y r]]
    1.92 -          (format
    1.93 -           "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
    1.94 -           p y r)))
    1.95 -       prop-data)))))
    1.96 +          (v-yaw (yaw y))
    1.97 +          (v-roll (roll r))
    1.98 +          (v-pitch (pitch p)))
    1.99 +        prop-data)))))
   1.100         
   1.101 +(comment
   1.102 +  (dorun
   1.103 +   (map
   1.104 +    (comp 
   1.105 +     println-repl
   1.106 +     (fn [[p y r]]
   1.107 +       (format
   1.108 +        "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
   1.109 +        p y r)))
   1.110 +    prop-data)))
   1.111 +
   1.112  
   1.113  
   1.114  
   1.115 @@ -359,11 +424,9 @@
   1.116                         (.mult (.getPhysicsRotation control)
   1.117                                (Vector3f. 1 0 0))))
   1.118  
   1.119 -       (if (= 0 (rem (swap! time inc) 20))
   1.120 -         (prop-view
   1.121 -          (joint-proprioception joint)))))))
   1.122 -            
   1.123 -            
   1.124 +       ;;(if (= 0 (rem (swap! time inc) 20))
   1.125 +         (prop-view (prop))))))
   1.126 +
   1.127  #+end_src
   1.128  
   1.129  #+results: test-body