# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1327999247 25200
# Node ID 3206d5e20beeeb865a9e09bcb15c10a44dc52056
# Parent  e98850b83c2c0fce61035b8f5a32787f61bc8e0f
still trying to fix proprioception

diff -r e98850b83c2c -r 3206d5e20bee org/body.org
--- a/org/body.org	Mon Jan 30 07:21:46 2012 -0700
+++ b/org/body.org	Tue Jan 31 01:40:47 2012 -0700
@@ -17,6 +17,15 @@
    com.jme3.math.Matrix3f
    com.jme3.bullet.control.RigidBodyControl))
 
+
+
+(defn quaternion-decompose [#^Quaternion q]
+  (map
+   #(arc-between % (.rotate q %))
+   [Vector3f/UNIT_X
+    Vector3f/UNIT_Y
+    Vector3f/UNIT_Z]))
+
 (defn any-orthogonal
   "Generate an arbitray (but stable) orthogonal vector to a given
    vector."
@@ -44,6 +53,30 @@
         beta  (.dot basis-2 (.project rotated basis-2))]
     (Math/atan2 beta alpha)))
 
+(defn right-handed? [vec1 vec2 vec3]
+  (< 0 (.dot (.cross vec1 vec2) vec3)))
+
+(defn project-quaternion
+  "From http://stackoverflow.com/questions/3684269/
+   component-of-a-quaternion-rotation-around-an-axis.
+
+   Determine the amount of rotation a quaternion will
+   cause about a given axis."
+  [#^Quaternion q #^Vector3f axis]
+  (let [axis (.normalize axis)
+        basis-1 (any-orthogonal axis)
+        basis-2 (.cross axis basis-1)
+        rotated (.mult q basis-1)
+        rotated-in-plane (.add (.project rotated basis-1)
+                               (.project rotated basis-2))]
+
+    ;; be sure to get sign from cross product
+    (if (right-handed? basis-1 rotated-in-plane axis)
+      (.angleBetween rotated-in-plane basis-1)
+      (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1)))))
+
+
+
 (defn joint-proprioception
   "Relative position information for a two-part system connected by a
    joint. Gives the pitch, yaw, and roll of the 'B' object relative to
@@ -89,6 +122,14 @@
     [pitch yaw roll]))
 
 
+
+
+(defn absolute-angle-between
+  [vec-1 vec-2]
+  
+
+
+
 (defn joint-proprioception
   [joint]
   (let [object-a (.getUserObject (.getBodyA joint))
@@ -104,12 +145,52 @@
          (.subtract 
           (.localToWorld object-b (.getPivotB joint) nil)
           (.getWorldTranslation object-b)))
+
+        rotate-a (.clone (.getWorldRotation object-a))
+        unrotate-a (.inverse (.getWorldRotation object-a))
+
+        canonical-arm-a (.mult unrotate-a arm-a)
+
+        basis-1 (any-orthogonal canonical-arm-a)
+        basis-2 (.normalize (.cross basis-1 canonical-arm-a))
+
+        pitch (.angleBetween arm-b basis-1)
+        yaw (.angleBetween arm-b basis-2)
+
+        twist-a
+        (project-quaternion
+         (.getWorldRotation object-a) arm-a)
         
-        
+        twist-b
+        (project-quaternion
+         (.getWorldRotation object-b) arm-b)
+
+        roll (- twist-b 0)
+        ;; "un-rotate" arm-a to it's canonical position, get two
+        ;; orthogonal basis vectors. Rotate those two vectors back to
+        ;; the correct position get the rotations between them.
+
+        ;; twist is the rotation about arm-a of obj-b minus the
+        ;; rotation about arm-a of obj-a
         ]
-    
-        
-   
+    ;; object-a == hand
+    ;; object-b == finger
+    [pitch yaw roll]))
+
+;; (defn joint-proprioception*
+;;   [joint]
+;;   (let [object-a (.getUserObject (.getBodyA joint))
+;;         object-b (.getUserObject (.getBodyB joint))
+
+;;         rotate-a (.clone (.getWorldRotation object-a))
+;;         rotate-b (.clone (.getWorldRotation object-b))
+
+;;         rotate-rel (.mult rotate-b (.inverse rotate-a))
+;;         ]
+;;     ((comp vec map) (partial project-quaternion rotate-rel)
+;;          [Vector3f/UNIT_X
+;;           Vector3f/UNIT_Y
+;;           Vector3f/UNIT_Z])))
 
 
 (defn proprioception
@@ -297,7 +378,7 @@
      (.setRGB image (+ u x) (+ v y) color))))
 
 (defn view-angle
-  "create a debug biew of an angle"
+  "create a debug view of an angle"
   [color]
   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
         previous (atom [25 25])
@@ -355,9 +436,9 @@
    and counterclockwise, and only affect roll."
   []
   (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
-                     :mass 0 :color ColorRGBA/Green)
+                     :mass 0 :color ColorRGBA/Green :name "hand")
         finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
-                    :mass 1 :color ColorRGBA/Red)
+                    :mass 1 :color ColorRGBA/Red :name "finger")
         floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
                      :mass 0 :color ColorRGBA/Gray)