1.1 --- a/org/body.org	Mon Jan 30 00:25:11 2012 -0700
     1.2 +++ b/org/body.org	Mon Jan 30 05:47:51 2012 -0700
     1.3 @@ -82,6 +82,7 @@
     1.4            (doto (Quaternion.)
     1.5              (.fromRotationMatrix rotate-a)))
     1.6           arm-b)]
     1.7 +    ;;(println-repl (.getName object-a) (.getName object-b))
     1.8      [pitch yaw roll]))
     1.9  
    1.10  (defn proprioception
    1.11 @@ -225,6 +226,61 @@
    1.12         (dorun (worm-motor-map
    1.13                 (worm-pattern @time)))))))
    1.14  
    1.15 +
    1.16 +(require 'cortex.silly)
    1.17 +(defn join-at-point [obj-a obj-b world-pivot]
    1.18 +  (cortex.silly/joint-dispatch
    1.19 +   {:type :point}
    1.20 +   (.getControl obj-a RigidBodyControl)
    1.21 +   (.getControl obj-b RigidBodyControl)
    1.22 +   (cortex.silly/world-to-local obj-a world-pivot)
    1.23 +   (cortex.silly/world-to-local obj-b world-pivot)
    1.24 +   nil
    1.25 +   ))
    1.26 +
    1.27 +
    1.28 +
    1.29 +(defn blab-* []
    1.30 +  (let [hand   (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
    1.31 +                     :mass 0 :color ColorRGBA/Green)
    1.32 +        finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
    1.33 +                    :mass 1 :color ColorRGBA/Red)
    1.34 +        connection-point (Vector3f. 1.2 0 0)
    1.35 +        root (nodify [hand finger])]
    1.36 +
    1.37 +    (join-at-point hand finger (Vector3f. 1.2 0 0))
    1.38 +    
    1.39 +    (.setCollisionGroup
    1.40 +     (.getControl hand RigidBodyControl)
    1.41 +     PhysicsCollisionObject/COLLISION_GROUP_NONE)
    1.42 +    (world
    1.43 +     root
    1.44 +     standard-debug-controls
    1.45 +     (fn [world]
    1.46 +       (enable-debug world)
    1.47 +       (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
    1.48 +       (set-gravity world Vector3f/ZERO)
    1.49 +       )
    1.50 +     no-op)))
    1.51 +     
    1.52 +(defn proprioception-debug-window
    1.53 +  []
    1.54 +  (let [vi (view-image)]
    1.55 +    (fn [prop-data]
    1.56 +      (dorun
    1.57 +       (map
    1.58 +       (comp 
    1.59 +        println-repl
    1.60 +        (fn [[p y r]]
    1.61 +          (format
    1.62 +           "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
    1.63 +           p y r)))
    1.64 +       prop-data)))))
    1.65 +       
    1.66 +
    1.67 +
    1.68 +
    1.69 +
    1.70  (defn test-proprioception
    1.71    "Testing proprioception:
    1.72     You should see two foating bars, and a printout of pitch, yaw, and
    1.73 @@ -236,7 +292,7 @@
    1.74    (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
    1.75                       :mass 0 :color ColorRGBA/Green)
    1.76          finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
    1.77 -                    :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
    1.78 +                    :mass 1 :color ColorRGBA/Red)
    1.79          floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
    1.80                       :mass 0 :color ColorRGBA/Gray)
    1.81  
    1.82 @@ -247,15 +303,23 @@
    1.83          roll-left? (atom false)
    1.84          roll-right? (atom false)
    1.85          control (.getControl finger RigidBodyControl)
    1.86 -        joint
    1.87 -        (doto 
    1.88 -            (Point2PointJoint.
    1.89 -             (.getControl hand RigidBodyControl)
    1.90 -             control
    1.91 -             (Vector3f. 1.2 0 0)
    1.92 -             (Vector3f. -1.2 0 0 ))
    1.93 -          (.setCollisionBetweenLinkedBodys false))
    1.94 -        time (atom 0)]
    1.95 +        time (atom 0)
    1.96 +        joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
    1.97 +        creature (nodify [hand finger])
    1.98 +        prop (proprioception creature)
    1.99 +
   1.100 +        prop-view (proprioception-debug-window)
   1.101 +
   1.102 +        
   1.103 +        ]
   1.104 +    
   1.105 +
   1.106 +    
   1.107 +    (.setCollisionGroup
   1.108 +     (.getControl hand RigidBodyControl)
   1.109 +     PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.110 +
   1.111 +    
   1.112      (world
   1.113       (nodify [hand finger floor])
   1.114       (merge standard-debug-controls
   1.115 @@ -266,10 +330,8 @@
   1.116               "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
   1.117               "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
   1.118       (fn [world]
   1.119 -       (.setTimer world (NanoTimer.))
   1.120 +       (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
   1.121         (set-gravity world (Vector3f. 0 0 0))
   1.122 -       (.setMoveSpeed (.getFlyByCamera world) 50)
   1.123 -       (.setRotationSpeed (.getFlyByCamera world) 50)
   1.124         (light-up-everything world))
   1.125       (fn [_ _]
   1.126         (if @move-up?
   1.127 @@ -297,15 +359,16 @@
   1.128                         (.mult (.getPhysicsRotation control)
   1.129                                (Vector3f. 1 0 0))))
   1.130  
   1.131 -       (if (= 0 (rem (swap! time inc) 2000))
   1.132 -         (do
   1.133 -           (apply
   1.134 -            (comp
   1.135 -             println-repl
   1.136 -             #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))
   1.137 -            (joint-proprioception joint))))))))
   1.138 +       (if (= 0 (rem (swap! time inc) 20))
   1.139 +         (prop-view
   1.140 +          (joint-proprioception joint)))))))
   1.141 +            
   1.142 +            
   1.143  #+end_src
   1.144  
   1.145 +#+results: test-body
   1.146 +: #'cortex.test.body/test-proprioception
   1.147 +
   1.148  
   1.149  * COMMENT code-limbo
   1.150  #+begin_src clojure