# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1327933306 25200
# Node ID e98850b83c2c0fce61035b8f5a32787f61bc8e0f
# Parent  b26017d1fe9a240d113de6a00ce1db45c6c565d5
going to fix error in proprioception

diff -r b26017d1fe9a -r e98850b83c2c org/body.org
--- a/org/body.org	Mon Jan 30 05:47:51 2012 -0700
+++ b/org/body.org	Mon Jan 30 07:21:46 2012 -0700
@@ -56,9 +56,12 @@
          (.subtract 
           (.localToWorld object-a (.getPivotA joint) nil)
           (.getWorldTranslation object-a)))
+
+        ;; this is probably wrong!
         rotate-a
          (doto (Matrix3f.)
            (.fromStartEndVectors arm-a Vector3f/UNIT_X))
+        
         arm-b
         (.mult
          rotate-a 
@@ -85,6 +88,30 @@
     ;;(println-repl (.getName object-a) (.getName object-b))
     [pitch yaw roll]))
 
+
+(defn joint-proprioception
+  [joint]
+  (let [object-a (.getUserObject (.getBodyA joint))
+        object-b (.getUserObject (.getBodyB joint))
+
+        arm-a
+        (.normalize
+         (.subtract 
+          (.localToWorld object-a (.getPivotA joint) nil)
+          (.getWorldTranslation object-a)))
+        arm-b
+        (.normalize
+         (.subtract 
+          (.localToWorld object-b (.getPivotB joint) nil)
+          (.getWorldTranslation object-b)))
+        
+        
+        ]
+    
+        
+   
+
+
 (defn proprioception
   "Create a function that provides proprioceptive information about an
   entire body."
@@ -262,21 +289,59 @@
        (set-gravity world Vector3f/ZERO)
        )
      no-op)))
-     
+(import java.awt.image.BufferedImage)
+
+(defn draw-sprite [image sprite x y color ]
+  (dorun
+   (for [[u v] sprite]
+     (.setRGB image (+ u x) (+ v y) color))))
+
+(defn view-angle
+  "create a debug biew of an angle"
+  [color]
+  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
+        previous (atom [25 25])
+        sprite [[0 0] [0 1]
+                [0 -1] [-1 0] [1 0]]] 
+    (fn [angle]
+      (let [angle (float angle)]
+        (let [position
+              [(+ 25 (int (* 20 (Math/cos angle))))
+               (+ 25 (int (* 20(Math/sin angle))))]]
+          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
+          (draw-sprite image sprite (position 0) (position 1) color)
+          (reset! previous position))
+        image))))
+
 (defn proprioception-debug-window
   []
-  (let [vi (view-image)]
+  (let [yaw (view-angle 0xFF0000)
+        pitch (view-angle 0x00FF00)
+        roll (view-angle 0xFFFFFF)
+        v-yaw (view-image)
+        v-pitch (view-image)
+        v-roll (view-image)
+        ]
     (fn [prop-data]
       (dorun
        (map
-       (comp 
-        println-repl
         (fn [[p y r]]
-          (format
-           "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
-           p y r)))
-       prop-data)))))
+          (v-yaw (yaw y))
+          (v-roll (roll r))
+          (v-pitch (pitch p)))
+        prop-data)))))
        
+(comment
+  (dorun
+   (map
+    (comp 
+     println-repl
+     (fn [[p y r]]
+       (format
+        "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
+        p y r)))
+    prop-data)))
+
 
 
 
@@ -359,11 +424,9 @@
                        (.mult (.getPhysicsRotation control)
                               (Vector3f. 1 0 0))))
 
-       (if (= 0 (rem (swap! time inc) 20))
-         (prop-view
-          (joint-proprioception joint)))))))
-            
-            
+       ;;(if (= 0 (rem (swap! time inc) 20))
+         (prop-view (prop))))))
+
 #+end_src
 
 #+results: test-body