# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1322474088 25200
# Node ID e5e627f50a3a46daec87b33c0892b1d8da922da2
# Parent  63951929fe44b745f9e1eebd285ab7d695304fb8
finally got euler angle stuff working

diff -r 63951929fe44 -r e5e627f50a3a assets/Models/property/test.blend
Binary file assets/Models/property/test.blend has changed
diff -r 63951929fe44 -r e5e627f50a3a org/body.org
--- a/org/body.org	Sat Nov 19 23:59:12 2011 -0700
+++ b/org/body.org	Mon Nov 28 02:54:48 2011 -0700
@@ -143,6 +143,20 @@
     node))
 
   
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+;; this could be a good way to give objects special properties like
+;; being eyes and the like
+
+(.getUserData 
+ (.getChild
+  (load-blender-model "Models/property/test.blend") 0)
+ "properties")
+
+;; the properties are saved along with the blender file.
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+
 
 (defn init-debug-skel-node
   [f debug-node skeleton]
@@ -312,32 +326,27 @@
   
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
+;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
 
 
-
-
-
-;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
-(defn joint-control
-  [joint]
-  (let [physics-space (ref nil)]
-    (reify PhysicsControl
-      (setPhysicsSpace [this space]
-        (dosync
-         (ref-set physics-space space))
-        (.addJoint space joint))
-      (update [this tpf])
-      (setSpatial [this spatial])
-      (render [this rm vp])
-      (getPhysicsSpace [this] (deref physics-space))
-      (isEnabled [this] true)
-      (setEnabled [this state]))))
-
+(defrecord JointControl [joint physics-space]
+  PhysicsControl
+  (setPhysicsSpace [this space]
+    (dosync
+     (ref-set (:physics-space this) space))
+    (.addJoint space (:joint this)))
+  (update [this tpf])
+  (setSpatial [this spatial])
+  (render [this rm vp])
+  (getPhysicsSpace [this] (deref (:physics-space this)))
+  (isEnabled [this] true)
+  (setEnabled [this state]))
+  
 (defn add-joint
   "Add a joint to a particular object. When the object is added to the
   PhysicsSpace of a simulation, the joint will also be added"
   [object joint]
-  (let [control (joint-control joint)]
+  (let [control (JointControl. joint (ref nil))]
     (.addControl object control))
   object)
 
@@ -371,7 +380,7 @@
 
 (defn nodify
   "take a sequence of things that can be attached to a node and return
-  a node with all of the attached"
+  a node with all of them attached"
   ([name children]
      (let [node (Node. name)]
        (dorun (map #(.attachChild node %) children))
@@ -427,19 +436,408 @@
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
 
+;;;;;;;;;  Mortor Control  ;;;;;;;;;;;;;
 
 
+;; surprisingly ehough, terristerial creatures only move by using
+;; torque applied to their joints.  There's not a single straight line
+;; of force in the human body at all!  (a straight line of force would
+;; correspond to some sort of jet or rocket propulseion)
 
+(defn node-seq
+  "take a node and return a seq of all its children
+   recursively. There will be no nodes left in the resulting
+   structure"
+  [#^Node node]
+  (tree-seq #(isa? (class %) Node) #(.getChildren %) node))
+  
+  
+(defn torque-controls [control]
+  (let [torques
+        (concat
+         (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
+              (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
+         [Vector3f/UNIT_X])]
+    (map (fn [torque-axis]
+           (fn [torque]
+             (.applyTorque
+              control
+              (.mult (.mult (.getPhysicsRotation control)
+                            torque-axis)
+                     (float
+                      (* (.getMass control) torque))))))
+         torques)))
 
+(defn motor-map
+  "Take a creature and generate a function that will enable fine
+   grained control over all the creature's limbs."  
+  [#^Node creature]
+  (let [controls (keep #(.getControl % RigidBodyControl)
+                       (node-seq creature))
+        limb-controls (reduce concat (map torque-controls controls))
+        body-control  (partial map #(%1 %2) limb-controls)]
+    body-control))
 
 
 
+(defn test-motor-map
+  "see how torque works."
+  []
+  (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
+                    :mass 1 :color ColorRGBA/Green)
+        motor-map (motor-map finger)]
+    (world
+     (nodify [finger
+              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+                   :color ColorRGBA/Gray)])
+     standard-debug-controls
+     (fn [world]
+       (set-gravity world Vector3f/ZERO)
+       (light-up-everything world)
+       (.setTimer world (NanoTimer.)))
+     (fn [_ _]
+       (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
+
+
+
+(defn test-torque
+  "see how torque works."
+  []
+  (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
+                    :mass 1 :color ColorRGBA/Green)
+        move-left? (atom false)
+        move-right? (atom false)
+        control (.getControl finger RigidBodyControl)]
+    (world
+     (nodify [finger
+              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+                   :color ColorRGBA/Gray)])
+     (merge standard-debug-controls
+            {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
+             "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
+     (fn [world]
+       (set-gravity world Vector3f/ZERO)
+       (light-up-everything world)
+       (.setTimer world (NanoTimer.)))
+     (fn [_ _]
+       (if @move-left?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                       (Vector3f. -3 20 0))))
+       (if @move-right?
+         (.applyTorque control (Vector3f. 0 0 1)))))))
+
+
+(defn worm-pattern [time]
+  [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+
+   0 0 0 0 0 0 0 0 0 0 0
+
+   (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
+
+   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0 0 0 0 0
+
+   ])
+
+
+;; (defn copier-gen []
+;;   (let [count (atom 0)]
+;;     (fn [in]
+;;       (swap! count inc)
+;;       (clojure.contrib.duck-streams/copy
+;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"
+;;                      ;;/home/r/tmp/mao-test/clojure-images
+;;                (format "%08d.png" @count)))))))
+;; (defn decrease-framerate []
+;;   (map
+;;    (copier-gen)
+;;    (sort
+;;     (map first
+;;          (partition
+;;           4
+;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))
+;;                   (file-seq
+;;                    (file-str
+;;                     "/home/r/media/anime/mao-temp/images"))))))))
+
+
+
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+
+
+
+;;;;;;;;;;;;;;;;;;  Proprioception  ;;;;;;;;;;;;;;;;;;;;;;;;
+
+
+;; this is not used as just getting the rotation would be simpler.
+(defn proprioception-senses 
+  "given a control , create a sequence of thunks that will report the
+  rotation of the control's object along the same axes as the motor-control map."
+  [control]
+  (let [torques
+        (concat
+         (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
+              (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
+         [Vector3f/UNIT_X])]
+    (map (fn [torque-axis]
+           (fn []
+             (.getPhysicsRotation control)))
+           torques)))
+
+(defn orthogonal-vect
+  "Return a vector orthogonal to the current one"
+  [vector]
+  (let [x (.getX vector)
+        y (.getY vector)
+        z (.getZ vector)]
+    (cond
+     (not= x (float 0)) (Vector3f. (- z) 0 x)
+     (not= y (float 0)) (Vector3f. 0 (- z) y)
+     (not= z (float 0)) (Vector3f. 0 (- z) y)
+     true Vector3f/ZERO)))
+
+;; from
+;; http://stackoverflow.com/questions/3684269/ \\
+;;   component-of-a-quaternion-rotation-around-an-axis
+(defn rot-about-axis [#^Quaternion q #^Vector3f axis]
+  (let [basis-1 (orthogonal-vect axis)
+        basis-2 (.cross axis basis-1)
+        rotated (.mult q basis-1)
+        alpha (.dot basis-1 (.project rotated basis-1))
+        beta  (.dot basis-2 (.project rotated basis-2))]
+    (println-repl alpha)
+    (println-repl beta)
+    (Math/atan2 beta alpha)))
+
+
+(defn check-rot [a]
+  (rot-about-axis
+   (doto (Quaternion.)
+     (.fromAngleAxis
+      (float a)
+      (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
+
+(defn relative-positions [joint]
+  (let [object-a (.getUserObject (.getBodyA joint))
+        object-b (.getUserObject (.getBodyB joint))
+        arm-a
+        (.normalize
+         (.subtract 
+          (.localToWorld object-a (.getPivotA joint) nil)
+          (.getWorldTranslation object-a)))
+        rotate-a
+         (doto (Matrix3f.)
+           (.fromStartEndVectors arm-a Vector3f/UNIT_X))
+        arm-b
+        (.mult
+         rotate-a 
+         (.normalize
+          (.subtract 
+           (.localToWorld object-b (.getPivotB joint) nil)
+           (.getWorldTranslation object-b))))
+        rotate-b
+        (doto (Matrix3f.)
+          (.fromStartEndVectors arm-b Vector3f/UNIT_X))
+        
+        pitch
+        (.angleBetween
+         (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
+         (Vector2f. 1 0))
+        yaw
+        (.angleBetween
+         (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
+         (Vector2f. 1 0))
+                     
+        roll
+        (.mult 
+         (.getLocalRotation object-b)
+         (doto (Quaternion.)
+           (.fromRotationMatrix rotate-a)))
+         ]
+         
+                
+
+    ;;(println-repl
+    ;; "arm-b is " arm-b)
+    ;;(println-repl
+    ;; "pivot-b is " (.getPivotB joint))
+    ;;(println-repl
+    ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
+    ;;         pitch yaw roll))
+    [pitch yaw roll]))
+
+        
+    
+        
+  
+(defn proprioception
+  "Create a proprioception map that reports the rotations of the
+  various limbs of the creature's body"
+  [creature]
+  [#^Node creature]
+  (let [
+        nodes (node-seq creature)
+        joints
+        (map
+         :joint 
+         (filter
+          #(isa? (class %) JointControl)
+          (reduce
+           concat
+           (map (fn [node]
+                  (map (fn [num] (.getControl node num))
+                       (range (.getNumControls node))))
+                nodes))))]
+    (fn []
+      (reduce concat (map relative-positions (list (first joints)))))))
+        
+
+(defn test-worm-control
+  []
+  (let [worm (point-worm)
+        time (atom 0)
+        worm-motor-map (motor-map worm)
+        body-map (proprioception worm)
+        debug-segments
+        (map
+         #(doto
+              (make-shape
+               (assoc base-shape
+                 :name "debug-line"
+                 :physical? false
+                 :shape
+                 (com.jme3.scene.shape.Line.
+                  (.add (.getWorldTranslation %)
+                        (Vector3f. -0.2 0 0 ))
+                  (.add (.getWorldTranslation %)
+                        (Vector3f. 0.2 0 0)))))
+                 (.setMaterial (green-x-ray)))
+            (drop 1 (node-seq worm)))]
+    (world
+     (nodify [worm
+              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+                   :color ColorRGBA/Gray)])
+     standard-debug-controls
+     (fn [world]
+       (.attachChild (.getRootNode world) (nodify debug-segments))
+       (enable-debug world)
+       (light-up-everything world)
+       (com.aurellem.capture.Capture/captureVideo
+        world
+        (file-str "/home/r/proj/cortex/tmp/moving-worm")))
+     
+     (fn [_ _]
+       (dorun
+        (map
+         (fn [worm-segment
+              debug-segment]
+           (.rotate
+            debug-segment
+            (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
+         (drop 1 (node-seq worm))
+         debug-segments))
+       (swap! time inc)
+       (println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))
+       (Thread/sleep 200)
+       (dorun (worm-motor-map
+               (worm-pattern @time)))))))
+
+
+
+
+
+(defn test-prop
+  "see how torque works."
+  []
+  (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
+                     :mass 0 :color ColorRGBA/Green)
+        finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
+                     :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
+        floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
+                     :mass 0 :color ColorRGBA/Gray)
+
+        move-up? (atom false)
+        move-down? (atom false)
+        move-left? (atom false)
+        move-right? (atom false)
+        roll-left? (atom false)
+        roll-right? (atom false)
+        control (.getControl finger RigidBodyControl)
+        joint
+        (doto 
+            (Point2PointJoint.
+             (.getControl hand RigidBodyControl)
+             control
+             (Vector3f. 1.2 0 0)
+             (Vector3f. -1.2 0 0 ))
+          (.setCollisionBetweenLinkedBodys false))
+        time (atom 0)
+        ]
+    (world
+     (nodify [hand finger floor])
+     (merge standard-debug-controls
+            {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
+             "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
+             "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
+             "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
+             "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
+             "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
+     (fn [world]
+       (set-gravity world Vector3f/ZERO)
+       (.setMoveSpeed (.getFlyByCamera world) 50)
+       (.setRotationSpeed (.getFlyByCamera world) 50)
+       (light-up-everything world)
+       (.setTimer world (NanoTimer.))
+       )
+     (fn [_ _]
+       (if @move-up?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                       (Vector3f. 0 0 1))))
+       (if @move-down?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                              (Vector3f. 0 0 -1))))
+       (if @move-left?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                              (Vector3f. 0 1 0))))
+       (if @move-right?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                              (Vector3f. 0 -1 0))))
+       (if @roll-left?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                              (Vector3f. -0.1 0 0))))
+       (if @roll-right?
+         (.applyTorque control
+                       (.mult (.getPhysicsRotation control)
+                              (Vector3f. 0.1 0 0))))
+
+       (if (= 0 (rem (swap! time inc) 2000))
+         (do 
+           ;;(println-repl (.getLocalRotation finger))
+           (println-repl (nth (relative-positions joint) 2))))
+
+       ))))
+
+
+
+
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
 
 
 
 #+end_src
 
+#+results: body-main
+: #'cortex.body/test-prop
+
 
 
 
@@ -452,3 +850,4 @@
 <<body-main>>
 #+end_src
   
+  
diff -r 63951929fe44 -r e5e627f50a3a org/util.org
--- a/org/util.org	Sat Nov 19 23:59:12 2011 -0700
+++ b/org/util.org	Mon Nov 28 02:54:48 2011 -0700
@@ -280,6 +280,8 @@
 *** Debug Actions
 #+srcname: debug-actions
 #+begin_src clojure :results silent
+(in-ns 'cortex.util)
+
 (defn basic-light-setup
   "returns a sequence of lights appropiate for fully lighting a scene"
   []
@@ -333,6 +335,13 @@
            (add-element game cannon-ball (if node node (.getRootNode game)))))))
   ([]
     (fire-cannon-ball false)))
+
+(def standard-debug-controls 
+  {"key-space" (fire-cannon-ball)})
+
+   
+
+
 #+end_src