1.1 --- a/org/body.org	Wed Feb 01 05:43:51 2012 -0700
     1.2 +++ b/org/body.org	Wed Feb 01 17:39:23 2012 -0700
     1.3 @@ -74,17 +74,18 @@
     1.4      (fn []
     1.5        ;; x is the "twist" axis, y and z are the "bend" axes
     1.6        (let [rot-a (.getWorldRotation obj-a)
     1.7 +            ;;inv-a (.inverse rot-a)
     1.8              rot-b (.getWorldRotation obj-b)
     1.9 -            relative (.mult rot-b (.inverse rot-a))
    1.10 +            ;;relative (.mult rot-b inv-a)
    1.11              basis (doto (Matrix3f.)
    1.12 -                    (.setColumn 0 x)
    1.13 -                    (.setColumn 1 y)
    1.14 -                    (.setColumn 2 z))
    1.15 +                    (.setColumn 0 (.mult rot-a x))
    1.16 +                    (.setColumn 1 (.mult rot-a y))
    1.17 +                    (.setColumn 2 (.mult rot-a z)))
    1.18              rotation-about-joint
    1.19              (doto (Quaternion.)
    1.20                (.fromRotationMatrix 
    1.21                 (.mult (.invert basis)
    1.22 -                      (.toRotationMatrix relative))))
    1.23 +                      (.toRotationMatrix rot-b))))
    1.24              [yaw roll pitch]
    1.25              (seq (.toAngles rotation-about-joint nil))]
    1.26          ;;return euler angles of the quaternion around the new basis            
    1.27 @@ -358,13 +359,16 @@
    1.28          joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
    1.29          creature (nodify [hand finger joint-node])
    1.30          ;; *******************************************
    1.31 +        rot (doto (Quaternion.)
    1.32 +               (.fromAngleAxis (/ Math/PI 4)
    1.33 +                               (Vector3f. 0 0 -1)))
    1.34          hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
    1.35 -                  :mass 0 :color ColorRGBA/Blue)
    1.36 +                  :mass 0 :color ColorRGBA/Blue :rotation rot)
    1.37          finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
    1.38 -                    :mass 1 :color ColorRGBA/Magenta)
    1.39 +                    :mass 1 :color ColorRGBA/Magenta :rotation rot)
    1.40          joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
    1.41                          :position (Vector3f. 1.2 1.5 -3)
    1.42 -                        :physical? false)
    1.43 +                        :physical? false :rotation rot)
    1.44          joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
    1.45          creature2 (nodify [hand2 finger2 joint-node2])
    1.46          ;; *******************************************
    1.47 @@ -383,14 +387,17 @@
    1.48          root (nodify [creature creature2 floor])
    1.49          prop (joint-proprioception creature joint-node)
    1.50          prop-view (proprioception-debug-window)]
    1.51 +             
    1.52      
    1.53  
    1.54 +    (.setCollisionGroup
    1.55 +     (.getControl hand RigidBodyControl)
    1.56 +     PhysicsCollisionObject/COLLISION_GROUP_NONE)
    1.57 +    (.setCollisionGroup
    1.58 +     (.getControl hand2 RigidBodyControl)
    1.59 +     PhysicsCollisionObject/COLLISION_GROUP_NONE)
    1.60 +
    1.61      
    1.62 -    (comment
    1.63 -      (.setCollisionGroup
    1.64 -       (.getControl hand RigidBodyControl)
    1.65 -       PhysicsCollisionObject/COLLISION_GROUP_NONE)
    1.66 -      )
    1.67  
    1.68      
    1.69      (world
    1.70 @@ -414,12 +421,18 @@
    1.71               down (Vector3f. 0 0 -1)
    1.72               roll-left (Vector3f. -1 0 0)
    1.73               roll-right (Vector3f. 1 0 0)]
    1.74 -         (if @move-up? (tap finger up force))
    1.75 -         (if @move-down? (tap finger down force))
    1.76 -         (if @move-left? (tap finger left force))
    1.77 -         (if @move-right? (tap finger right force))
    1.78 -         (if @roll-left? (tap finger roll-left (/ force 10)))
    1.79 -         (if @roll-right? (tap finger roll-right (/ force 10))))
    1.80 +         (if @move-up? (do (tap finger up force)
    1.81 +                           (tap finger2 up force)))
    1.82 +         (if @move-down? (do (tap finger down force)
    1.83 +                             (tap finger2 down force)))
    1.84 +         (if @move-left? (do (tap finger left force)
    1.85 +                             (tap finger2 left force)))
    1.86 +         (if @move-right? (do (tap finger right force)
    1.87 +                              (tap finger2 right force)))
    1.88 +         (if @roll-left? (do (tap finger roll-left (/ force 10))
    1.89 +                             (tap finger2 roll-left (/ force 10))))
    1.90 +         (if @roll-right? (do (tap finger roll-right (/ force 10))
    1.91 +                              (tap finger2 roll-right (/ force 10)))))
    1.92         (prop-view (list (prop)))))))
    1.93  
    1.94  #+end_src