1.1 --- a/org/body.org	Wed Feb 01 02:44:07 2012 -0700
     1.2 +++ b/org/body.org	Wed Feb 01 05:43:51 2012 -0700
     1.3 @@ -15,23 +15,67 @@
     1.4     com.jme3.math.Quaternion
     1.5     com.jme3.math.Vector2f
     1.6     com.jme3.math.Matrix3f
     1.7 -   com.jme3.bullet.control.RigidBodyControl))
     1.8 +   com.jme3.bullet.control.RigidBodyControl
     1.9 +   com.jme3.collision.CollisionResults
    1.10 +   com.jme3.bounding.BoundingBox))
    1.11  
    1.12  (import com.jme3.scene.Node)
    1.13  
    1.14 +(defn jme-to-blender
    1.15 +  "Convert from JME coordinates to Blender coordinates"
    1.16 +  [#^Vector3f in]
    1.17 +  (Vector3f. (.getX in)
    1.18 +             (- (.getZ in))
    1.19 +             (.getY in)))
    1.20 +
    1.21 +(defn joint-targets
    1.22 +  "Return the two closest two objects to the joint object, ordered
    1.23 +  from bottom to top according to the joint's rotation."
    1.24 +  [#^Node parts #^Node joint]
    1.25 +  (loop [radius (float 0.01)]
    1.26 +    (let [results (CollisionResults.)]
    1.27 +      (.collideWith
    1.28 +       parts
    1.29 +       (BoundingBox. (.getWorldTranslation joint)
    1.30 +                     radius radius radius)
    1.31 +       results)
    1.32 +      (let [targets
    1.33 +            (distinct
    1.34 +             (map  #(.getGeometry %) results))]
    1.35 +        (if (>= (count targets) 2)
    1.36 +          (sort-by
    1.37 +           #(let [v
    1.38 +                  (jme-to-blender
    1.39 +                   (.mult
    1.40 +                    (.inverse (.getWorldRotation joint))
    1.41 +                    (.subtract (.getWorldTranslation %)
    1.42 +                               (.getWorldTranslation joint))))]
    1.43 +              (println-repl (.getName %) ":" v)
    1.44 +              (.dot (Vector3f. 1 1 1)
    1.45 +                    v))                  
    1.46 +           (take 2 targets))
    1.47 +          (recur (float (* radius 2))))))))
    1.48 +
    1.49 +(defn creature-joints 
    1.50 +  "Return the children of the creature's \"joints\" node."
    1.51 +  [#^Node creature]
    1.52 +  (if-let [joint-node (.getChild creature "joints")]
    1.53 +    (seq (.getChildren joint-node))
    1.54 +    (do (println-repl "could not find JOINTS node") [])))
    1.55 +
    1.56  (defn joint-proprioception [#^Node parts #^Node joint]
    1.57 -  (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
    1.58 +  (let [[obj-a obj-b] (joint-targets parts joint)
    1.59          joint-rot (.getWorldRotation joint)
    1.60          x (.mult joint-rot Vector3f/UNIT_X)
    1.61          y (.mult joint-rot Vector3f/UNIT_Y)
    1.62          z (.mult joint-rot Vector3f/UNIT_Z)]
    1.63      ;; this function will report proprioceptive information for the
    1.64 -    ;; joint 
    1.65 +    ;; joint.
    1.66      (fn []
    1.67        ;; x is the "twist" axis, y and z are the "bend" axes
    1.68        (let [rot-a (.getWorldRotation obj-a)
    1.69              rot-b (.getWorldRotation obj-b)
    1.70 -            relative (.mult (.inverse rot-a) rot-b)
    1.71 +            relative (.mult rot-b (.inverse rot-a))
    1.72              basis (doto (Matrix3f.)
    1.73                      (.setColumn 0 x)
    1.74                      (.setColumn 1 y)
    1.75 @@ -44,7 +88,6 @@
    1.76              [yaw roll pitch]
    1.77              (seq (.toAngles rotation-about-joint nil))]
    1.78          ;;return euler angles of the quaternion around the new basis            
    1.79 -        ;;[yaw pitch roll]
    1.80          [yaw roll pitch]
    1.81          ))))
    1.82  
    1.83 @@ -54,7 +97,7 @@
    1.84    entire body."
    1.85    [#^Node creature]
    1.86    ;; extract the body's joints
    1.87 -  (let [joints (cortex.silly/creature-joints creature)
    1.88 +  (let [joints (creature-joints creature)
    1.89          senses (map (partial joint-proprioception creature) joints)]
    1.90      (fn []
    1.91        (map #(%) senses))))
    1.92 @@ -95,6 +138,7 @@
    1.93  #+begin_src clojure
    1.94  (ns cortex.test.body
    1.95    (:use (cortex world util body))
    1.96 +  (:require cortex.silly)
    1.97    (:import
    1.98     com.jme3.math.Vector3f
    1.99     com.jme3.math.ColorRGBA
   1.100 @@ -188,7 +232,7 @@
   1.101                 (worm-pattern @time)))))))
   1.102  
   1.103  
   1.104 -(require 'cortex.silly)
   1.105 +
   1.106  (defn join-at-point [obj-a obj-b world-pivot]
   1.107    (cortex.silly/joint-dispatch
   1.108     {:type :point}
   1.109 @@ -287,6 +331,13 @@
   1.110  
   1.111  
   1.112  
   1.113 +(defn tap [obj direction force]
   1.114 +  (let [control (.getControl obj RigidBodyControl)]
   1.115 +    (.applyTorque
   1.116 +     control
   1.117 +     (.mult (.getPhysicsRotation control)
   1.118 +            (.mult (.normalize direction) (float force))))))
   1.119 +
   1.120  
   1.121  
   1.122  (defn test-proprioception
   1.123 @@ -301,11 +352,25 @@
   1.124                       :mass 0 :color ColorRGBA/Green :name "hand")
   1.125          finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   1.126                      :mass 1 :color ColorRGBA/Red :name "finger")
   1.127 -        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.128 -                     :mass 0 :color ColorRGBA/Gray)
   1.129          joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
   1.130                          :position (Vector3f. 1.2 2 0)
   1.131                          :physical? false)
   1.132 +        joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   1.133 +        creature (nodify [hand finger joint-node])
   1.134 +        ;; *******************************************
   1.135 +        hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
   1.136 +                  :mass 0 :color ColorRGBA/Blue)
   1.137 +        finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
   1.138 +                    :mass 1 :color ColorRGBA/Magenta)
   1.139 +        joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
   1.140 +                        :position (Vector3f. 1.2 1.5 -3)
   1.141 +                        :physical? false)
   1.142 +        joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
   1.143 +        creature2 (nodify [hand2 finger2 joint-node2])
   1.144 +        ;; *******************************************
   1.145 +        
   1.146 +        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.147 +                     :mass 0 :color ColorRGBA/Gray)
   1.148  
   1.149          move-up? (atom false)
   1.150          move-down? (atom false)
   1.151 @@ -313,27 +378,23 @@
   1.152          move-right? (atom false)
   1.153          roll-left? (atom false)
   1.154          roll-right? (atom false)
   1.155 -        control (.getControl finger RigidBodyControl)
   1.156 -        time (atom 0)
   1.157 -        joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   1.158 -        creature (nodify [hand finger joint-node])
   1.159 +       
   1.160 +
   1.161 +        root (nodify [creature creature2 floor])
   1.162          prop (joint-proprioception creature joint-node)
   1.163 -
   1.164 -        prop-view (proprioception-debug-window)
   1.165 -
   1.166 -        
   1.167 -        ]
   1.168 +        prop-view (proprioception-debug-window)]
   1.169      
   1.170  
   1.171      
   1.172 -
   1.173 -    (.setCollisionGroup
   1.174 -     (.getControl hand RigidBodyControl)
   1.175 -     PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.176 +    (comment
   1.177 +      (.setCollisionGroup
   1.178 +       (.getControl hand RigidBodyControl)
   1.179 +       PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.180 +      )
   1.181  
   1.182      
   1.183      (world
   1.184 -     (nodify [hand finger floor joint-node])
   1.185 +     root
   1.186       (merge standard-debug-controls
   1.187              {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.188               "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.189 @@ -346,33 +407,20 @@
   1.190         (set-gravity world (Vector3f. 0 0 0))
   1.191         (light-up-everything world))
   1.192       (fn [_ _]
   1.193 -       (if @move-up?
   1.194 -         (.applyTorque control
   1.195 -                       (.mult (.getPhysicsRotation control)
   1.196 -                       (Vector3f. 0 0 10))))
   1.197 -       (if @move-down?
   1.198 -         (.applyTorque control
   1.199 -                       (.mult (.getPhysicsRotation control)
   1.200 -                              (Vector3f. 0 0 -10))))
   1.201 -       (if @move-left?
   1.202 -         (.applyTorque control
   1.203 -                       (.mult (.getPhysicsRotation control)
   1.204 -                              (Vector3f. 0 10 0))))
   1.205 -       (if @move-right?
   1.206 -         (.applyTorque control
   1.207 -                       (.mult (.getPhysicsRotation control)
   1.208 -                              (Vector3f. 0 -10 0))))
   1.209 -       (if @roll-left?
   1.210 -         (.applyTorque control
   1.211 -                       (.mult (.getPhysicsRotation control)
   1.212 -                              (Vector3f. -1 0 0))))
   1.213 -       (if @roll-right?
   1.214 -         (.applyTorque control
   1.215 -                       (.mult (.getPhysicsRotation control)
   1.216 -                              (Vector3f. 1 0 0))))
   1.217 -
   1.218 -       ;;(if (= 0 (rem (swap! time inc) 20))
   1.219 -         (prop-view (list (prop)))))))
   1.220 +       (let [force 10
   1.221 +             left (Vector3f. 0 1 0)
   1.222 +             right (Vector3f. 0 -1 0)
   1.223 +             up (Vector3f. 0 0 1)
   1.224 +             down (Vector3f. 0 0 -1)
   1.225 +             roll-left (Vector3f. -1 0 0)
   1.226 +             roll-right (Vector3f. 1 0 0)]
   1.227 +         (if @move-up? (tap finger up force))
   1.228 +         (if @move-down? (tap finger down force))
   1.229 +         (if @move-left? (tap finger left force))
   1.230 +         (if @move-right? (tap finger right force))
   1.231 +         (if @roll-left? (tap finger roll-left (/ force 10)))
   1.232 +         (if @roll-right? (tap finger roll-right (/ force 10))))
   1.233 +       (prop-view (list (prop)))))))
   1.234  
   1.235  #+end_src
   1.236  

     2.1 --- a/org/test-creature.org	Wed Feb 01 02:44:07 2012 -0700
     2.2 +++ b/org/test-creature.org	Wed Feb 01 05:43:51 2012 -0700
     2.3 @@ -262,73 +262,7 @@
     2.4               (.getZ in)
     2.5               (- (.getY in))))
     2.6  
     2.7 -(defn jme-to-blender
     2.8 -  "Convert from JME coordinates to Blender coordinates"
     2.9 -  [#^Vector3f in]
    2.10 -  (Vector3f. (.getX in)
    2.11 -             (- (.getZ in))
    2.12 -             (.getY in)))
    2.13  
    2.14 -(defn joint-targets
    2.15 -  "Return the two closest two objects to the joint object, ordered
    2.16 -  from bottom to top according to the joint's rotation."
    2.17 -  [#^Node parts #^Node joint]
    2.18 -  (loop [radius (float 0.01)]
    2.19 -    (let [results (CollisionResults.)]
    2.20 -      (.collideWith
    2.21 -       parts
    2.22 -       (BoundingBox. (.getWorldTranslation joint)
    2.23 -                     radius radius radius)
    2.24 -       results)
    2.25 -      (let [targets
    2.26 -            (distinct
    2.27 -             (map  #(.getGeometry %) results))]
    2.28 -        (if (>= (count targets) 2)
    2.29 -          (sort-by
    2.30 -           #(let [v
    2.31 -                  (jme-to-blender
    2.32 -                   (.mult
    2.33 -                    (.inverse (.getWorldRotation joint))
    2.34 -                    (.subtract (.getWorldTranslation %)
    2.35 -                               (.getWorldTranslation joint))))]
    2.36 -              (println-repl (.getName %) ":" v)
    2.37 -              (.dot (Vector3f. 1 1 1)
    2.38 -                    v))                  
    2.39 -           (take 2 targets))
    2.40 -          (recur (float (* radius 2))))))))
    2.41 -
    2.42 -
    2.43 -(defn proprio-joint [#^Node parts #^Node joint]
    2.44 -  (let [[obj-a obj-b] (joint-targets parts joint)
    2.45 -        joint-rot (.getWorldRotation joint)
    2.46 -        x (.mult joint-rot Vector3f/UNIT_X)
    2.47 -        y (.mult joint-rot Vector3f/UNIT_Y)
    2.48 -        z (.mult joint-rot Vector3f/UNIT_Z)]
    2.49 -    ;; this function will report proprioceptive information for the
    2.50 -    ;; joint 
    2.51 -    (fn []
    2.52 -      ;; x is the "twist" axis, y and z are the "bend" axes
    2.53 -      (let [rot-a (.getWorldRotation obj-a)
    2.54 -            rot-b (.getWorldRotation obj-b)
    2.55 -            relative (.mult (.inverse rot-a) rot-b)
    2.56 -            basis (doto (Matrix3f.)
    2.57 -                    (.setColumn 0 y)
    2.58 -                    (.setColumn 1 z)
    2.59 -                    (.setColumn 2 x))
    2.60 -            rotation-about-joint
    2.61 -            (doto (Quaternion.)
    2.62 -              (.fromRotationMatrix 
    2.63 -               (.mult (.inverse basis)
    2.64 -                      (.toRotationMatrix relative))))
    2.65 -
    2.66 -            confirm-axes
    2.67 -            (let [temp-axes (make-array Vector3f 3)]
    2.68 -              (.toAxes rotation-about-joint temp-axes)
    2.69 -              (seq temp-axes))
    2.70 -            euler-angles
    2.71 -            (seq (.toAngles rotation-about-joint nil))]
    2.72 -        ;;return euler angles of the quaternion around the new basis            
    2.73 -        euler-angles))))
    2.74  
    2.75  
    2.76  
    2.77 @@ -1058,12 +992,6 @@
    2.78  ;; these are the functions that provide world i/o, chinese-room style
    2.79  
    2.80  
    2.81 -(defn creature-joints 
    2.82 -  "Return the children of the creature's \"joints\" node."
    2.83 -  [#^Node creature]
    2.84 -  (if-let [joint-node (.getChild creature "joints")]
    2.85 -    (seq (.getChildren joint-node))
    2.86 -    (do (println-repl "could not find JOINTS node") [])))
    2.87  
    2.88  
    2.89  (defn blender-creature
    2.90 @@ -1156,24 +1084,6 @@
    2.91         (.getRGB image x 0)))))
    2.92  
    2.93  
    2.94 -(defn rad->deg [rad]
    2.95 -  (* 180 (/ Math/PI) rad))
    2.96 -
    2.97 -
    2.98 -(defn debug-prop-window
    2.99 -  "create a debug view for proprioception"
   2.100 -  []
   2.101 -  (let [vi (view-image)]
   2.102 -    (fn [sensor-data]
   2.103 -      (println-repl
   2.104 -       (map
   2.105 -        (fn [[yaw pitch roll]]
   2.106 -          [(rad->deg yaw)
   2.107 -           (rad->deg pitch)
   2.108 -           (rad->deg roll)])
   2.109 -        sensor-data)))))
   2.110 -
   2.111 -
   2.112  (defn draw-sprite [image sprite x y color ]
   2.113    (dorun
   2.114     (for [[u v] sprite]
   2.115 @@ -1239,7 +1149,7 @@
   2.116          bell (AudioNode. (asset-manager)
   2.117                           "Sounds/pure.wav" false)
   2.118          prop (proprioception creature)
   2.119 -        prop-debug (debug-prop-window)
   2.120 +        prop-debug (proprioception-debug-window)
   2.121          ;; dream
   2.122  
   2.123          ]