1.1 --- a/org/body.org	Mon Jan 30 07:21:46 2012 -0700
     1.2 +++ b/org/body.org	Tue Jan 31 01:40:47 2012 -0700
     1.3 @@ -17,6 +17,15 @@
     1.4     com.jme3.math.Matrix3f
     1.5     com.jme3.bullet.control.RigidBodyControl))
     1.6  
     1.7 +
     1.8 +
     1.9 +(defn quaternion-decompose [#^Quaternion q]
    1.10 +  (map
    1.11 +   #(arc-between % (.rotate q %))
    1.12 +   [Vector3f/UNIT_X
    1.13 +    Vector3f/UNIT_Y
    1.14 +    Vector3f/UNIT_Z]))
    1.15 +
    1.16  (defn any-orthogonal
    1.17    "Generate an arbitray (but stable) orthogonal vector to a given
    1.18     vector."
    1.19 @@ -44,6 +53,30 @@
    1.20          beta  (.dot basis-2 (.project rotated basis-2))]
    1.21      (Math/atan2 beta alpha)))
    1.22  
    1.23 +(defn right-handed? [vec1 vec2 vec3]
    1.24 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
    1.25 +
    1.26 +(defn project-quaternion
    1.27 +  "From http://stackoverflow.com/questions/3684269/
    1.28 +   component-of-a-quaternion-rotation-around-an-axis.
    1.29 +
    1.30 +   Determine the amount of rotation a quaternion will
    1.31 +   cause about a given axis."
    1.32 +  [#^Quaternion q #^Vector3f axis]
    1.33 +  (let [axis (.normalize axis)
    1.34 +        basis-1 (any-orthogonal axis)
    1.35 +        basis-2 (.cross axis basis-1)
    1.36 +        rotated (.mult q basis-1)
    1.37 +        rotated-in-plane (.add (.project rotated basis-1)
    1.38 +                               (.project rotated basis-2))]
    1.39 +
    1.40 +    ;; be sure to get sign from cross product
    1.41 +    (if (right-handed? basis-1 rotated-in-plane axis)
    1.42 +      (.angleBetween rotated-in-plane basis-1)
    1.43 +      (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1)))))
    1.44 +
    1.45 +
    1.46 +
    1.47  (defn joint-proprioception
    1.48    "Relative position information for a two-part system connected by a
    1.49     joint. Gives the pitch, yaw, and roll of the 'B' object relative to
    1.50 @@ -89,6 +122,14 @@
    1.51      [pitch yaw roll]))
    1.52  
    1.53  
    1.54 +
    1.55 +
    1.56 +(defn absolute-angle-between
    1.57 +  [vec-1 vec-2]
    1.58 +  
    1.59 +
    1.60 +
    1.61 +
    1.62  (defn joint-proprioception
    1.63    [joint]
    1.64    (let [object-a (.getUserObject (.getBodyA joint))
    1.65 @@ -104,12 +145,52 @@
    1.66           (.subtract 
    1.67            (.localToWorld object-b (.getPivotB joint) nil)
    1.68            (.getWorldTranslation object-b)))
    1.69 +
    1.70 +        rotate-a (.clone (.getWorldRotation object-a))
    1.71 +        unrotate-a (.inverse (.getWorldRotation object-a))
    1.72 +
    1.73 +        canonical-arm-a (.mult unrotate-a arm-a)
    1.74 +
    1.75 +        basis-1 (any-orthogonal canonical-arm-a)
    1.76 +        basis-2 (.normalize (.cross basis-1 canonical-arm-a))
    1.77 +
    1.78 +        pitch (.angleBetween arm-b basis-1)
    1.79 +        yaw (.angleBetween arm-b basis-2)
    1.80 +
    1.81 +        twist-a
    1.82 +        (project-quaternion
    1.83 +         (.getWorldRotation object-a) arm-a)
    1.84          
    1.85 -        
    1.86 +        twist-b
    1.87 +        (project-quaternion
    1.88 +         (.getWorldRotation object-b) arm-b)
    1.89 +
    1.90 +        roll (- twist-b 0)
    1.91 +        ;; "un-rotate" arm-a to it's canonical position, get two
    1.92 +        ;; orthogonal basis vectors. Rotate those two vectors back to
    1.93 +        ;; the correct position get the rotations between them.
    1.94 +
    1.95 +        ;; twist is the rotation about arm-a of obj-b minus the
    1.96 +        ;; rotation about arm-a of obj-a
    1.97          ]
    1.98 -    
    1.99 -        
   1.100 -   
   1.101 +    ;; object-a == hand
   1.102 +    ;; object-b == finger
   1.103 +    [pitch yaw roll]))
   1.104 +
   1.105 +;; (defn joint-proprioception*
   1.106 +;;   [joint]
   1.107 +;;   (let [object-a (.getUserObject (.getBodyA joint))
   1.108 +;;         object-b (.getUserObject (.getBodyB joint))
   1.109 +
   1.110 +;;         rotate-a (.clone (.getWorldRotation object-a))
   1.111 +;;         rotate-b (.clone (.getWorldRotation object-b))
   1.112 +
   1.113 +;;         rotate-rel (.mult rotate-b (.inverse rotate-a))
   1.114 +;;         ]
   1.115 +;;     ((comp vec map) (partial project-quaternion rotate-rel)
   1.116 +;;          [Vector3f/UNIT_X
   1.117 +;;           Vector3f/UNIT_Y
   1.118 +;;           Vector3f/UNIT_Z])))
   1.119  
   1.120  
   1.121  (defn proprioception
   1.122 @@ -297,7 +378,7 @@
   1.123       (.setRGB image (+ u x) (+ v y) color))))
   1.124  
   1.125  (defn view-angle
   1.126 -  "create a debug biew of an angle"
   1.127 +  "create a debug view of an angle"
   1.128    [color]
   1.129    (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
   1.130          previous (atom [25 25])
   1.131 @@ -355,9 +436,9 @@
   1.132     and counterclockwise, and only affect roll."
   1.133    []
   1.134    (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
   1.135 -                     :mass 0 :color ColorRGBA/Green)
   1.136 +                     :mass 0 :color ColorRGBA/Green :name "hand")
   1.137          finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   1.138 -                    :mass 1 :color ColorRGBA/Red)
   1.139 +                    :mass 1 :color ColorRGBA/Red :name "finger")
   1.140          floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
   1.141                       :mass 0 :color ColorRGBA/Gray)
   1.142