1.1 --- a/org/body.org	Fri Feb 03 06:15:34 2012 -0700
     1.2 +++ b/org/body.org	Fri Feb 03 06:21:39 2012 -0700
     1.3 @@ -62,61 +62,6 @@
     1.4      (seq (.getChildren joint-node))
     1.5      (do (println-repl "could not find JOINTS node") [])))
     1.6  
     1.7 -(defn right-handed? [vec1 vec2 vec3]
     1.8 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
     1.9 -
    1.10 -(defn absolute-angle [vec1 vec2 axis]
    1.11 -  (let [angle (.angleBetween vec1 vec2)]
    1.12 -    (if (right-handed? vec1 vec2 axis)
    1.13 -      angle (- (* 2 Math/PI) angle))))
    1.14 -
    1.15 -
    1.16 -(defn joint-proprioception [#^Node parts #^Node joint]
    1.17 -  (let [[obj-a obj-b] (joint-targets parts joint)
    1.18 -        joint-rot (.getWorldRotation joint)
    1.19 -        x0 (.mult joint-rot Vector3f/UNIT_X)
    1.20 -        y0 (.mult joint-rot Vector3f/UNIT_Y)
    1.21 -        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    1.22 -    (println-repl "x:" x0)
    1.23 -    (println-repl "y:" y0)
    1.24 -    (println-repl "z:" z0)
    1.25 -    (println-repl "init-a:" (.getWorldRotation obj-a))
    1.26 -    (println-repl "init-b:" (.getWorldRotation obj-b))
    1.27 -
    1.28 -    (fn []
    1.29 -      (let [rot-a (.clone (.getWorldRotation obj-a))
    1.30 -            rot-b (.clone (.getWorldRotation obj-b))
    1.31 -            x (.mult rot-a x0)
    1.32 -            y (.mult rot-a y0)
    1.33 -            z (.mult rot-a z0)
    1.34 -
    1.35 -            X (.mult rot-b x0)
    1.36 -            Y (.mult rot-b y0)
    1.37 -            Z (.mult rot-b z0)
    1.38 -            heading  (Math/atan2 (.dot X z) (.dot X x))
    1.39 -            pitch  (Math/atan2 (.dot X y) (.dot X x))
    1.40 -
    1.41 -            ;; rotate x-vector back to origin
    1.42 -            reverse
    1.43 -            (doto (Quaternion.)
    1.44 -              (.fromAngleAxis
    1.45 -               (.angleBetween X x)
    1.46 -               (let [cross (.normalize (.cross X x))]
    1.47 -                 (if (= 0 (.length cross)) y cross))))
    1.48 -            roll (absolute-angle (.mult reverse Y) y x)]
    1.49 -        
    1.50 -        [heading pitch roll]))))
    1.51 -
    1.52 -(defn proprioception
    1.53 -  "Create a function that provides proprioceptive information about an
    1.54 -  entire body."
    1.55 -  [#^Node creature]
    1.56 -  ;; extract the body's joints
    1.57 -  (let [joints (creature-joints creature)
    1.58 -        senses (map (partial joint-proprioception creature) joints)]
    1.59 -    (fn []
    1.60 -      (map #(%) senses))))
    1.61 -
    1.62  (defn tap [obj direction force]
    1.63    (let [control (.getControl obj RigidBodyControl)]
    1.64      (.applyTorque