1.1 --- a/org/body.org	Wed Feb 01 17:39:23 2012 -0700
     1.2 +++ b/org/body.org	Wed Feb 01 23:46:31 2012 -0700
     1.3 @@ -66,9 +66,13 @@
     1.4  (defn joint-proprioception [#^Node parts #^Node joint]
     1.5    (let [[obj-a obj-b] (joint-targets parts joint)
     1.6          joint-rot (.getWorldRotation joint)
     1.7 -        x (.mult joint-rot Vector3f/UNIT_X)
     1.8 -        y (.mult joint-rot Vector3f/UNIT_Y)
     1.9 -        z (.mult joint-rot Vector3f/UNIT_Z)]
    1.10 +        pre-inv-a (.inverse (.getWorldRotation obj-a))
    1.11 +        x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
    1.12 +        y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
    1.13 +        z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))]
    1.14 +    (println-repl "x:" x)
    1.15 +    (println-repl "y:" y)
    1.16 +    (println-repl "z:" z)
    1.17      ;; this function will report proprioceptive information for the
    1.18      ;; joint.
    1.19      (fn []
    1.20 @@ -341,6 +345,64 @@
    1.21  
    1.22  
    1.23  
    1.24 +(defmacro with-movement
    1.25 +  [object
    1.26 +   [up down left right roll-up roll-down :as keyboard]
    1.27 +   forces
    1.28 +   [world-invocation
    1.29 +    root-node
    1.30 +    keymap
    1.31 +    intilization
    1.32 +    world-loop]]
    1.33 +  (let [add-keypress
    1.34 +        (fn [state keymap key]
    1.35 +          `(merge ~keymap
    1.36 +                  {~key
    1.37 +                   (fn [_ pressed?#]
    1.38 +                     (reset! ~state pressed?#))}))
    1.39 +        move-left? (gensym "move-left?")
    1.40 +        move-right? (gensym "move-right?")
    1.41 +        move-up? (gensym "move-up?")
    1.42 +        move-down? (gensym "move-down?")
    1.43 +        roll-left? (gensym "roll-left?")
    1.44 +        roll-right? (gensym "roll-right?")
    1.45 +        directions
    1.46 +        [(Vector3f. 0 1 0)
    1.47 +         (Vector3f. 0 -1 0)
    1.48 +         (Vector3f. 0 0 1)
    1.49 +         (Vector3f. 0 0 -1)
    1.50 +         (Vector3f. -1 0 0)
    1.51 +         (Vector3f. 1 0 0)]
    1.52 +        symbols [move-left? move-right? move-up? move-down? 
    1.53 +                 roll-left? roll-right?]
    1.54 +
    1.55 +        keymap* (vec (map #(add-keypress %1 keymap %2)
    1.56 +                          symbols
    1.57 +                          keyboard))
    1.58 +        
    1.59 +        splice-loop (map (fn [sym force direction]
    1.60 +                           `(if (deref ~sym)
    1.61 +                              (tap ~object ~direction ~force)))
    1.62 +                         symbols directions forces)
    1.63 +
    1.64 +        world-loop* `(fn [world# tpf#]
    1.65 +                       (~world-loop world# tpf#)
    1.66 +                       ~@splice-loop)]
    1.67 +        
    1.68 +  `(let [~move-up? (atom false)
    1.69 +         ~move-down? (atom false)
    1.70 +         ~move-left? (atom false)
    1.71 +         ~move-right? (atom false)
    1.72 +         ~roll-left? (atom false)
    1.73 +         ~roll-right? (atom false)]
    1.74 +     (~world-invocation
    1.75 +      ~root-node
    1.76 +      (reduce merge ~keymap*)
    1.77 +      ~intilization
    1.78 +      ~world-loop*)
    1.79 +  )))
    1.80 +
    1.81 +
    1.82  (defn test-proprioception
    1.83    "Testing proprioception:
    1.84     You should see two foating bars, and a printout of pitch, yaw, and
    1.85 @@ -359,23 +421,10 @@
    1.86          joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
    1.87          creature (nodify [hand finger joint-node])
    1.88          ;; *******************************************
    1.89 -        rot (doto (Quaternion.)
    1.90 -               (.fromAngleAxis (/ Math/PI 4)
    1.91 -                               (Vector3f. 0 0 -1)))
    1.92 -        hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
    1.93 -                  :mass 0 :color ColorRGBA/Blue :rotation rot)
    1.94 -        finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
    1.95 -                    :mass 1 :color ColorRGBA/Magenta :rotation rot)
    1.96 -        joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
    1.97 -                        :position (Vector3f. 1.2 1.5 -3)
    1.98 -                        :physical? false :rotation rot)
    1.99 -        joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
   1.100 -        creature2 (nodify [hand2 finger2 joint-node2])
   1.101 -        ;; *******************************************
   1.102 -        
   1.103 +                
   1.104          floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.105                       :mass 0 :color ColorRGBA/Gray)
   1.106 -
   1.107 +        
   1.108          move-up? (atom false)
   1.109          move-down? (atom false)
   1.110          move-left? (atom false)
   1.111 @@ -384,7 +433,7 @@
   1.112          roll-right? (atom false)
   1.113         
   1.114  
   1.115 -        root (nodify [creature creature2 floor])
   1.116 +        root (nodify [creature floor])
   1.117          prop (joint-proprioception creature joint-node)
   1.118          prop-view (proprioception-debug-window)]
   1.119               
   1.120 @@ -393,48 +442,46 @@
   1.121      (.setCollisionGroup
   1.122       (.getControl hand RigidBodyControl)
   1.123       PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.124 -    (.setCollisionGroup
   1.125 -     (.getControl hand2 RigidBodyControl)
   1.126 -     PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.127  
   1.128 -    
   1.129 -
   1.130 -    
   1.131 -    (world
   1.132 -     root
   1.133 -     (merge standard-debug-controls
   1.134 -            {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.135 -             "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.136 -             "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
   1.137 -             "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
   1.138 -             "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
   1.139 -             "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
   1.140 -     (fn [world]
   1.141 -       (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
   1.142 -       (set-gravity world (Vector3f. 0 0 0))
   1.143 -       (light-up-everything world))
   1.144 -     (fn [_ _]
   1.145 -       (let [force 10
   1.146 -             left (Vector3f. 0 1 0)
   1.147 -             right (Vector3f. 0 -1 0)
   1.148 -             up (Vector3f. 0 0 1)
   1.149 -             down (Vector3f. 0 0 -1)
   1.150 -             roll-left (Vector3f. -1 0 0)
   1.151 -             roll-right (Vector3f. 1 0 0)]
   1.152 -         (if @move-up? (do (tap finger up force)
   1.153 -                           (tap finger2 up force)))
   1.154 -         (if @move-down? (do (tap finger down force)
   1.155 -                             (tap finger2 down force)))
   1.156 -         (if @move-left? (do (tap finger left force)
   1.157 -                             (tap finger2 left force)))
   1.158 -         (if @move-right? (do (tap finger right force)
   1.159 -                              (tap finger2 right force)))
   1.160 -         (if @roll-left? (do (tap finger roll-left (/ force 10))
   1.161 -                             (tap finger2 roll-left (/ force 10))))
   1.162 -         (if @roll-right? (do (tap finger roll-right (/ force 10))
   1.163 -                              (tap finger2 roll-right (/ force 10)))))
   1.164 -       (prop-view (list (prop)))))))
   1.165 -
   1.166 +    (with-movement
   1.167 +      finger
   1.168 +      ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
   1.169 +      [10 10 10 10 1 1]
   1.170 +      (world
   1.171 +       root
   1.172 +       (merge standard-debug-controls
   1.173 +              {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.174 +               "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.175 +               "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
   1.176 +               "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
   1.177 +               "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
   1.178 +               "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
   1.179 +       (fn [world]
   1.180 +         (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
   1.181 +         (set-gravity world (Vector3f. 0 0 0))
   1.182 +         (light-up-everything world))
   1.183 +       (fn [_ _]
   1.184 +         (let [force 10
   1.185 +               left (Vector3f. 0 1 0)
   1.186 +               right (Vector3f. 0 -1 0)
   1.187 +               up (Vector3f. 0 0 1)
   1.188 +               down (Vector3f. 0 0 -1)
   1.189 +               roll-left (Vector3f. -1 0 0)
   1.190 +               roll-right (Vector3f. 1 0 0)]
   1.191 +           (if @move-up?  (tap finger up force))
   1.192 +           
   1.193 +           (if @move-down?  (tap finger down force))
   1.194 +           
   1.195 +           (if @move-left?  (tap finger left force))
   1.196 +           
   1.197 +           (if @move-right?  (tap finger right force))
   1.198 +           
   1.199 +           (if @roll-left?  (tap finger roll-left (/ force 10)))
   1.200 +           
   1.201 +           (if @roll-right?  (tap finger roll-right (/ force 10))))
   1.202 +         
   1.203 +         (prop-view (list (prop))))))))
   1.204 +  
   1.205  #+end_src
   1.206  
   1.207  #+results: test-body