1.1 --- a/org/body.org	Wed Feb 01 02:27:18 2012 -0700
     1.2 +++ b/org/body.org	Wed Feb 01 02:44:07 2012 -0700
     1.3 @@ -17,144 +17,6 @@
     1.4     com.jme3.math.Matrix3f
     1.5     com.jme3.bullet.control.RigidBodyControl))
     1.6  
     1.7 -(comment
     1.8 -  (defn joint-proprioception
     1.9 -  "Relative position information for a two-part system connected by a
    1.10 -   joint. Gives the pitch, yaw, and roll of the 'B' object relative to
    1.11 -   the 'A' object, as determined by the joint."
    1.12 -  [joint]
    1.13 -  (let [object-a (.getUserObject (.getBodyA joint))
    1.14 -        object-b (.getUserObject (.getBodyB joint))
    1.15 -        arm-a
    1.16 -        (.normalize
    1.17 -         (.subtract 
    1.18 -          (.localToWorld object-a (.getPivotA joint) nil)
    1.19 -          (.getWorldTranslation object-a)))
    1.20 -
    1.21 -        ;; this is probably wrong!
    1.22 -        rotate-a
    1.23 -         (doto (Matrix3f.)
    1.24 -           (.fromStartEndVectors arm-a Vector3f/UNIT_X))
    1.25 -        
    1.26 -        arm-b
    1.27 -        (.mult
    1.28 -         rotate-a 
    1.29 -         (.normalize
    1.30 -          (.subtract 
    1.31 -           (.localToWorld object-b (.getPivotB joint) nil)
    1.32 -           (.getWorldTranslation object-b))))
    1.33 -        pitch
    1.34 -        (.angleBetween
    1.35 -         (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
    1.36 -         (Vector2f. 1 0))
    1.37 -        yaw
    1.38 -        (.angleBetween
    1.39 -         (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
    1.40 -         (Vector2f. 1 0))
    1.41 -                     
    1.42 -        roll
    1.43 -        (project-quaternion
    1.44 -         (.mult 
    1.45 -          (.getLocalRotation object-b)
    1.46 -          (doto (Quaternion.)
    1.47 -            (.fromRotationMatrix rotate-a)))
    1.48 -         arm-b)]
    1.49 -    ;;(println-repl (.getName object-a) (.getName object-b))
    1.50 -    [pitch yaw roll]))
    1.51 -)
    1.52 -
    1.53 -(defn any-orthogonal
    1.54 -  "Generate an arbitray (but stable) orthogonal vector to a given
    1.55 -   vector."
    1.56 -  [vector]
    1.57 -  (let [x (.getX vector)
    1.58 -        y (.getY vector)
    1.59 -        z (.getZ vector)]
    1.60 -    (cond
    1.61 -     (not= x (float 0)) (Vector3f. (- z) 0 x)
    1.62 -     (not= y (float 0)) (Vector3f. 0 (- z) y)
    1.63 -     (not= z (float 0)) (Vector3f. 0 (- z) y)
    1.64 -     true Vector3f/ZERO)))
    1.65 -
    1.66 -(comment
    1.67 -(defn project-quaternion
    1.68 -  "From http://stackoverflow.com/questions/3684269/
    1.69 -   component-of-a-quaternion-rotation-around-an-axis.
    1.70 -
    1.71 -   Determine the amount of rotation a quaternion will
    1.72 -   cause about a given axis."
    1.73 -  [#^Quaternion q #^Vector3f axis]
    1.74 -  (let [basis-1 (any-orthogonal axis)
    1.75 -        basis-2 (.cross axis basis-1)
    1.76 -        rotated (.mult q basis-1)
    1.77 -        alpha (.dot basis-1 (.project rotated basis-1))
    1.78 -        beta  (.dot basis-2 (.project rotated basis-2))]
    1.79 -    (Math/atan2 beta alpha)))
    1.80 -)
    1.81 -
    1.82 -(defn right-handed? [vec1 vec2 vec3]
    1.83 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
    1.84 -
    1.85 -(defn absolute-angle [vec1 vec2 axis]
    1.86 -  (let [angle (.angleBetween vec1 vec2)]
    1.87 -    (if (right-handed? vec1 vec2 axis)
    1.88 -      angle (- (* 2 Math/PI) angle))))
    1.89 -
    1.90 -(defn angle-min [& angles]
    1.91 -  (first
    1.92 -   (sort-by 
    1.93 -    (fn [angle]
    1.94 -      (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))]
    1.95 -        (min in-circle
    1.96 -             (- (* Math/PI 2) in-circle))))
    1.97 -    angles)))
    1.98 -
    1.99 -(defn project-quaternion
   1.100 -  "From http://stackoverflow.com/questions/3684269/
   1.101 -   component-of-a-quaternion-rotation-around-an-axis.
   1.102 -
   1.103 -   Determine the amount of rotation a quaternion will
   1.104 -   cause about a given axis."
   1.105 -  [#^Quaternion q #^Vector3f axis]
   1.106 -  (let [axis (.normalize axis)
   1.107 -        basis-1 (.normalize (any-orthogonal axis))
   1.108 -        basis-2 (.cross axis basis-1)
   1.109 -        rotated-1 (.mult q basis-1)
   1.110 -        basis-1* (.normalize
   1.111 -                          (.add (.project rotated-1 basis-1)
   1.112 -                                (.project rotated-1 basis-2)))
   1.113 -        rotated-2 (.mult q basis-2)
   1.114 -        basis-2* (.normalize
   1.115 -                          (.add (.project rotated-2 basis-1)
   1.116 -                                (.project rotated-2 basis-2)))
   1.117 -        angle-1
   1.118 -        (absolute-angle basis-1 basis-1* axis)
   1.119 -        angle-2
   1.120 -        (absolute-angle basis-2 basis-2* axis)
   1.121 -
   1.122 -
   1.123 -        angle (angle-min angle-1 angle-2)
   1.124 -        ]
   1.125 -  
   1.126 -
   1.127 -    ;; be sure to get sign from cross product
   1.128 -    (if false
   1.129 -      (do
   1.130 -        (println-repl "axis" axis)
   1.131 -        (println-repl "basis-1" basis-1)
   1.132 -        (println-repl "basis-2" basis-2)
   1.133 -        (println-repl "rotated-1" rotated-1)
   1.134 -        (println-repl "rotated-2" rotated-2)
   1.135 -        (println-repl "basis-1*" basis-1*)
   1.136 -        (println-repl "basis-2*" basis-2*)
   1.137 -        (println-repl "angle-1" angle-1)
   1.138 -        (println-repl "angle-2" angle-2)
   1.139 -        
   1.140 -        (println-repl "angle" angle)
   1.141 -        (println-repl "")))
   1.142 -    angle))
   1.143 -    
   1.144 -
   1.145  (import com.jme3.scene.Node)
   1.146  
   1.147  (defn joint-proprioception [#^Node parts #^Node joint]
   1.148 @@ -187,53 +49,15 @@
   1.149          ))))
   1.150  
   1.151  
   1.152 -(comment
   1.153 -
   1.154 -(defn joint-proprioception
   1.155 -  [joint]
   1.156 -  (let [object-a (.getUserObject (.getBodyA joint))
   1.157 -        object-b (.getUserObject (.getBodyB joint))
   1.158 -        rot-a (.clone (.getWorldRotation object-a))
   1.159 -        rot-b (.clone (.getWorldRotation object-b))
   1.160 -        ]
   1.161 -    
   1.162 -    (.mult rot-b (.inverse rot-a))
   1.163 -    
   1.164 -    ;; object-a == hand
   1.165 -    ;; object-b == finger
   1.166 -    ))
   1.167 -)
   1.168 -;; (defn joint-proprioception*
   1.169 -;;   [joint]
   1.170 -;;   (let [object-a (.getUserObject (.getBodyA joint))
   1.171 -;;         object-b (.getUserObject (.getBodyB joint))
   1.172 -
   1.173 -;;         rotate-a (.clone (.getWorldRotation object-a))
   1.174 -;;         rotate-b (.clone (.getWorldRotation object-b))
   1.175 -
   1.176 -;;         rotate-rel (.mult rotate-b (.inverse rotate-a))
   1.177 -;;         ]
   1.178 -;;     ((comp vec map) (partial project-quaternion rotate-rel)
   1.179 -;;          [Vector3f/UNIT_X
   1.180 -;;           Vector3f/UNIT_Y
   1.181 -;;           Vector3f/UNIT_Z])))
   1.182 -
   1.183 -
   1.184  (defn proprioception
   1.185    "Create a function that provides proprioceptive information about an
   1.186    entire body."
   1.187 -  [body]
   1.188 +  [#^Node creature]
   1.189    ;; extract the body's joints
   1.190 -  (let [joints 
   1.191 -        (distinct
   1.192 -         (reduce
   1.193 -          concat
   1.194 -          (map #(.getJoints %)
   1.195 -               (keep
   1.196 -                #(.getControl % RigidBodyControl)
   1.197 -                (node-seq body)))))]
   1.198 +  (let [joints (cortex.silly/creature-joints creature)
   1.199 +        senses (map (partial joint-proprioception creature) joints)]
   1.200      (fn []
   1.201 -      (map joint-proprioception joints))))
   1.202 +      (map #(%) senses))))
   1.203    
   1.204  #+end_src
   1.205