1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/org/proprioception.org	Fri Feb 03 06:21:39 2012 -0700
     1.3 @@ -0,0 +1,87 @@
     1.4 +#+title: The Sense of Proprioception
     1.5 +#+author: Robert McIntyre
     1.6 +#+email: rlm@mit.edu
     1.7 +#+description: proprioception for simulated creatures
     1.8 +#+keywords: simulation, jMonkeyEngine3, clojure
     1.9 +#+SETUPFILE: ../../aurellem/org/setup.org
    1.10 +#+INCLUDE: ../../aurellem/org/level-0.org
    1.11 +
    1.12 +#+name: proprioception
    1.13 +#+begin_src clojure
    1.14 +(ns cortex.proprioception
    1.15 +  (:use (cortex world util sense body)))
    1.16 +
    1.17 +(cortex.import/mega-import-jme3)
    1.18 +
    1.19 +(defn right-handed? [vec1 vec2 vec3]
    1.20 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
    1.21 +
    1.22 +(defn absolute-angle [vec1 vec2 axis]
    1.23 +  (let [angle (.angleBetween vec1 vec2)]
    1.24 +    (if (right-handed? vec1 vec2 axis)
    1.25 +      angle (- (* 2 Math/PI) angle))))
    1.26 +
    1.27 +
    1.28 +(defn joint-proprioception [#^Node parts #^Node joint]
    1.29 +  (let [[obj-a obj-b] (joint-targets parts joint)
    1.30 +        joint-rot (.getWorldRotation joint)
    1.31 +        x0 (.mult joint-rot Vector3f/UNIT_X)
    1.32 +        y0 (.mult joint-rot Vector3f/UNIT_Y)
    1.33 +        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    1.34 +    (println-repl "x:" x0)
    1.35 +    (println-repl "y:" y0)
    1.36 +    (println-repl "z:" z0)
    1.37 +    (println-repl "init-a:" (.getWorldRotation obj-a))
    1.38 +    (println-repl "init-b:" (.getWorldRotation obj-b))
    1.39 +
    1.40 +    (fn []
    1.41 +      (let [rot-a (.clone (.getWorldRotation obj-a))
    1.42 +            rot-b (.clone (.getWorldRotation obj-b))
    1.43 +            x (.mult rot-a x0)
    1.44 +            y (.mult rot-a y0)
    1.45 +            z (.mult rot-a z0)
    1.46 +
    1.47 +            X (.mult rot-b x0)
    1.48 +            Y (.mult rot-b y0)
    1.49 +            Z (.mult rot-b z0)
    1.50 +            heading  (Math/atan2 (.dot X z) (.dot X x))
    1.51 +            pitch  (Math/atan2 (.dot X y) (.dot X x))
    1.52 +
    1.53 +            ;; rotate x-vector back to origin
    1.54 +            reverse
    1.55 +            (doto (Quaternion.)
    1.56 +              (.fromAngleAxis
    1.57 +               (.angleBetween X x)
    1.58 +               (let [cross (.normalize (.cross X x))]
    1.59 +                 (if (= 0 (.length cross)) y cross))))
    1.60 +            roll (absolute-angle (.mult reverse Y) y x)]
    1.61 +        
    1.62 +        [heading pitch roll]))))
    1.63 +
    1.64 +(defn proprioception
    1.65 +  "Create a function that provides proprioceptive information about an
    1.66 +  entire body."
    1.67 +  [#^Node creature]
    1.68 +  ;; extract the body's joints
    1.69 +  (let [joints (creature-joints creature)
    1.70 +        senses (map (partial joint-proprioception creature) joints)]
    1.71 +    (fn []
    1.72 +      (map #(%) senses))))
    1.73 +
    1.74 +
    1.75 +
    1.76 +
    1.77 +
    1.78 +#+end_src
    1.79 +
    1.80 +
    1.81 +
    1.82 +
    1.83 +
    1.84 +
    1.85 +
    1.86 +
    1.87 +* COMMENT generate source
    1.88 +#+begin_src clojure :tangle ../src/cortex/proprioception.clj
    1.89 +<<proprioception>>
    1.90 +#+end_src