1.1 --- a/org/test-creature.org	Tue Jan 31 01:40:47 2012 -0700
     1.2 +++ b/org/test-creature.org	Wed Feb 01 02:27:18 2012 -0700
     1.3 @@ -297,6 +297,41 @@
     1.4             (take 2 targets))
     1.5            (recur (float (* radius 2))))))))
     1.6  
     1.7 +
     1.8 +(defn proprio-joint [#^Node parts #^Node joint]
     1.9 +  (let [[obj-a obj-b] (joint-targets parts joint)
    1.10 +        joint-rot (.getWorldRotation joint)
    1.11 +        x (.mult joint-rot Vector3f/UNIT_X)
    1.12 +        y (.mult joint-rot Vector3f/UNIT_Y)
    1.13 +        z (.mult joint-rot Vector3f/UNIT_Z)]
    1.14 +    ;; this function will report proprioceptive information for the
    1.15 +    ;; joint 
    1.16 +    (fn []
    1.17 +      ;; x is the "twist" axis, y and z are the "bend" axes
    1.18 +      (let [rot-a (.getWorldRotation obj-a)
    1.19 +            rot-b (.getWorldRotation obj-b)
    1.20 +            relative (.mult (.inverse rot-a) rot-b)
    1.21 +            basis (doto (Matrix3f.)
    1.22 +                    (.setColumn 0 y)
    1.23 +                    (.setColumn 1 z)
    1.24 +                    (.setColumn 2 x))
    1.25 +            rotation-about-joint
    1.26 +            (doto (Quaternion.)
    1.27 +              (.fromRotationMatrix 
    1.28 +               (.mult (.inverse basis)
    1.29 +                      (.toRotationMatrix relative))))
    1.30 +
    1.31 +            confirm-axes
    1.32 +            (let [temp-axes (make-array Vector3f 3)]
    1.33 +              (.toAxes rotation-about-joint temp-axes)
    1.34 +              (seq temp-axes))
    1.35 +            euler-angles
    1.36 +            (seq (.toAngles rotation-about-joint nil))]
    1.37 +        ;;return euler angles of the quaternion around the new basis            
    1.38 +        euler-angles))))
    1.39 +
    1.40 +
    1.41 +
    1.42  (defn world-to-local
    1.43     "Convert the world coordinates into coordinates relative to the 
    1.44     object (i.e. local coordinates), taking into account the rotation
    1.45 @@ -457,8 +492,7 @@
    1.46    (dorun
    1.47     (map
    1.48      (fn [joint]
    1.49 -      (let [[obj-a obj-b]
    1.50 -            (joint-targets pieces joint)]
    1.51 +      (let [[obj-a obj-b] (joint-targets pieces joint)]
    1.52          (connect obj-a obj-b joint)))
    1.53      joints))
    1.54    pieces)
    1.55 @@ -1188,7 +1222,7 @@
    1.56                (comp (view-image) BufferedImage!))
    1.57       
    1.58       (add-eye world (.getCamera world) no-op)
    1.59 -     
    1.60 +     ;;(set-gravity world (Vector3f. 0 0 0))
    1.61       ;;(com.aurellem.capture.Capture/captureVideo
    1.62       ;; world (file-str "/home/r/proj/ai-videos/hand"))
    1.63       ;;(.setTimer world (RatchetTimer. 60))
    1.64 @@ -1323,7 +1357,7 @@
    1.65  #+end_src
    1.66  
    1.67  #+results: body-1
    1.68 -: #'cortex.silly/test-creature
    1.69 +: #'cortex.silly/follow-test
    1.70  
    1.71  
    1.72  * COMMENT purgatory