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