# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1328275299 25200
# Node ID 84c67be00abe4d37a51a9b5156c7d132d83d7b6f
# Parent  e8df6e76c3e5f02c6c3922ce2a146e4118026d58
refactored proprioception

diff -r e8df6e76c3e5 -r 84c67be00abe org/body.org
--- a/org/body.org	Fri Feb 03 06:15:34 2012 -0700
+++ b/org/body.org	Fri Feb 03 06:21:39 2012 -0700
@@ -62,61 +62,6 @@
     (seq (.getChildren joint-node))
     (do (println-repl "could not find JOINTS node") [])))
 
-(defn right-handed? [vec1 vec2 vec3]
-  (< 0 (.dot (.cross vec1 vec2) vec3)))
-
-(defn absolute-angle [vec1 vec2 axis]
-  (let [angle (.angleBetween vec1 vec2)]
-    (if (right-handed? vec1 vec2 axis)
-      angle (- (* 2 Math/PI) angle))))
-
-
-(defn joint-proprioception [#^Node parts #^Node joint]
-  (let [[obj-a obj-b] (joint-targets parts joint)
-        joint-rot (.getWorldRotation joint)
-        x0 (.mult joint-rot Vector3f/UNIT_X)
-        y0 (.mult joint-rot Vector3f/UNIT_Y)
-        z0 (.mult joint-rot Vector3f/UNIT_Z)]
-    (println-repl "x:" x0)
-    (println-repl "y:" y0)
-    (println-repl "z:" z0)
-    (println-repl "init-a:" (.getWorldRotation obj-a))
-    (println-repl "init-b:" (.getWorldRotation obj-b))
-
-    (fn []
-      (let [rot-a (.clone (.getWorldRotation obj-a))
-            rot-b (.clone (.getWorldRotation obj-b))
-            x (.mult rot-a x0)
-            y (.mult rot-a y0)
-            z (.mult rot-a z0)
-
-            X (.mult rot-b x0)
-            Y (.mult rot-b y0)
-            Z (.mult rot-b z0)
-            heading  (Math/atan2 (.dot X z) (.dot X x))
-            pitch  (Math/atan2 (.dot X y) (.dot X x))
-
-            ;; rotate x-vector back to origin
-            reverse
-            (doto (Quaternion.)
-              (.fromAngleAxis
-               (.angleBetween X x)
-               (let [cross (.normalize (.cross X x))]
-                 (if (= 0 (.length cross)) y cross))))
-            roll (absolute-angle (.mult reverse Y) y x)]
-        
-        [heading pitch roll]))))
-
-(defn proprioception
-  "Create a function that provides proprioceptive information about an
-  entire body."
-  [#^Node creature]
-  ;; extract the body's joints
-  (let [joints (creature-joints creature)
-        senses (map (partial joint-proprioception creature) joints)]
-    (fn []
-      (map #(%) senses))))
-
 (defn tap [obj direction force]
   (let [control (.getControl obj RigidBodyControl)]
     (.applyTorque
diff -r e8df6e76c3e5 -r 84c67be00abe org/proprioception.org
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/org/proprioception.org	Fri Feb 03 06:21:39 2012 -0700
@@ -0,0 +1,87 @@
+#+title: The Sense of Proprioception
+#+author: Robert McIntyre
+#+email: rlm@mit.edu
+#+description: proprioception for simulated creatures
+#+keywords: simulation, jMonkeyEngine3, clojure
+#+SETUPFILE: ../../aurellem/org/setup.org
+#+INCLUDE: ../../aurellem/org/level-0.org
+
+#+name: proprioception
+#+begin_src clojure
+(ns cortex.proprioception
+  (:use (cortex world util sense body)))
+
+(cortex.import/mega-import-jme3)
+
+(defn right-handed? [vec1 vec2 vec3]
+  (< 0 (.dot (.cross vec1 vec2) vec3)))
+
+(defn absolute-angle [vec1 vec2 axis]
+  (let [angle (.angleBetween vec1 vec2)]
+    (if (right-handed? vec1 vec2 axis)
+      angle (- (* 2 Math/PI) angle))))
+
+
+(defn joint-proprioception [#^Node parts #^Node joint]
+  (let [[obj-a obj-b] (joint-targets parts joint)
+        joint-rot (.getWorldRotation joint)
+        x0 (.mult joint-rot Vector3f/UNIT_X)
+        y0 (.mult joint-rot Vector3f/UNIT_Y)
+        z0 (.mult joint-rot Vector3f/UNIT_Z)]
+    (println-repl "x:" x0)
+    (println-repl "y:" y0)
+    (println-repl "z:" z0)
+    (println-repl "init-a:" (.getWorldRotation obj-a))
+    (println-repl "init-b:" (.getWorldRotation obj-b))
+
+    (fn []
+      (let [rot-a (.clone (.getWorldRotation obj-a))
+            rot-b (.clone (.getWorldRotation obj-b))
+            x (.mult rot-a x0)
+            y (.mult rot-a y0)
+            z (.mult rot-a z0)
+
+            X (.mult rot-b x0)
+            Y (.mult rot-b y0)
+            Z (.mult rot-b z0)
+            heading  (Math/atan2 (.dot X z) (.dot X x))
+            pitch  (Math/atan2 (.dot X y) (.dot X x))
+
+            ;; rotate x-vector back to origin
+            reverse
+            (doto (Quaternion.)
+              (.fromAngleAxis
+               (.angleBetween X x)
+               (let [cross (.normalize (.cross X x))]
+                 (if (= 0 (.length cross)) y cross))))
+            roll (absolute-angle (.mult reverse Y) y x)]
+        
+        [heading pitch roll]))))
+
+(defn proprioception
+  "Create a function that provides proprioceptive information about an
+  entire body."
+  [#^Node creature]
+  ;; extract the body's joints
+  (let [joints (creature-joints creature)
+        senses (map (partial joint-proprioception creature) joints)]
+    (fn []
+      (map #(%) senses))))
+
+
+
+
+
+#+end_src
+
+
+
+
+
+
+
+
+* COMMENT generate source
+#+begin_src clojure :tangle ../src/cortex/proprioception.clj
+<<proprioception>>
+#+end_src
diff -r e8df6e76c3e5 -r 84c67be00abe org/test-creature.org
--- a/org/test-creature.org	Fri Feb 03 06:15:34 2012 -0700
+++ b/org/test-creature.org	Fri Feb 03 06:21:39 2012 -0700
@@ -49,7 +49,7 @@
 ;; TODO remove this!
 (require 'cortex.import)
 (cortex.import/mega-import-jme3)
-(use '(cortex world util body hearing touch vision sense))
+(use '(cortex world util body hearing touch vision sense proprioception))
 
 (rlm.rlm-commands/help)
 (import java.awt.image.BufferedImage)