# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1328143163 25200
# Node ID 47a4d74761f02f87a692819f22f9f3b10df75dcb
# Parent  421cc43441ae7b90c9899dc5ccf67a16a3403ad3
fixed some more problems with proprioception

diff -r 421cc43441ae -r 47a4d74761f0 assets/Models/creature1/try-again.blend
Binary file assets/Models/creature1/try-again.blend has changed
diff -r 421cc43441ae -r 47a4d74761f0 org/body.org
--- a/org/body.org	Wed Feb 01 05:43:51 2012 -0700
+++ b/org/body.org	Wed Feb 01 17:39:23 2012 -0700
@@ -74,17 +74,18 @@
     (fn []
       ;; x is the "twist" axis, y and z are the "bend" axes
       (let [rot-a (.getWorldRotation obj-a)
+            ;;inv-a (.inverse rot-a)
             rot-b (.getWorldRotation obj-b)
-            relative (.mult rot-b (.inverse rot-a))
+            ;;relative (.mult rot-b inv-a)
             basis (doto (Matrix3f.)
-                    (.setColumn 0 x)
-                    (.setColumn 1 y)
-                    (.setColumn 2 z))
+                    (.setColumn 0 (.mult rot-a x))
+                    (.setColumn 1 (.mult rot-a y))
+                    (.setColumn 2 (.mult rot-a z)))
             rotation-about-joint
             (doto (Quaternion.)
               (.fromRotationMatrix 
                (.mult (.invert basis)
-                      (.toRotationMatrix relative))))
+                      (.toRotationMatrix rot-b))))
             [yaw roll pitch]
             (seq (.toAngles rotation-about-joint nil))]
         ;;return euler angles of the quaternion around the new basis            
@@ -358,13 +359,16 @@
         joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
         creature (nodify [hand finger joint-node])
         ;; *******************************************
+        rot (doto (Quaternion.)
+               (.fromAngleAxis (/ Math/PI 4)
+                               (Vector3f. 0 0 -1)))
         hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
-                  :mass 0 :color ColorRGBA/Blue)
+                  :mass 0 :color ColorRGBA/Blue :rotation rot)
         finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
-                    :mass 1 :color ColorRGBA/Magenta)
+                    :mass 1 :color ColorRGBA/Magenta :rotation rot)
         joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
                         :position (Vector3f. 1.2 1.5 -3)
-                        :physical? false)
+                        :physical? false :rotation rot)
         joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
         creature2 (nodify [hand2 finger2 joint-node2])
         ;; *******************************************
@@ -383,14 +387,17 @@
         root (nodify [creature creature2 floor])
         prop (joint-proprioception creature joint-node)
         prop-view (proprioception-debug-window)]
+             
     
 
+    (.setCollisionGroup
+     (.getControl hand RigidBodyControl)
+     PhysicsCollisionObject/COLLISION_GROUP_NONE)
+    (.setCollisionGroup
+     (.getControl hand2 RigidBodyControl)
+     PhysicsCollisionObject/COLLISION_GROUP_NONE)
+
     
-    (comment
-      (.setCollisionGroup
-       (.getControl hand RigidBodyControl)
-       PhysicsCollisionObject/COLLISION_GROUP_NONE)
-      )
 
     
     (world
@@ -414,12 +421,18 @@
              down (Vector3f. 0 0 -1)
              roll-left (Vector3f. -1 0 0)
              roll-right (Vector3f. 1 0 0)]
-         (if @move-up? (tap finger up force))
-         (if @move-down? (tap finger down force))
-         (if @move-left? (tap finger left force))
-         (if @move-right? (tap finger right force))
-         (if @roll-left? (tap finger roll-left (/ force 10)))
-         (if @roll-right? (tap finger roll-right (/ force 10))))
+         (if @move-up? (do (tap finger up force)
+                           (tap finger2 up force)))
+         (if @move-down? (do (tap finger down force)
+                             (tap finger2 down force)))
+         (if @move-left? (do (tap finger left force)
+                             (tap finger2 left force)))
+         (if @move-right? (do (tap finger right force)
+                              (tap finger2 right force)))
+         (if @roll-left? (do (tap finger roll-left (/ force 10))
+                             (tap finger2 roll-left (/ force 10))))
+         (if @roll-right? (do (tap finger roll-right (/ force 10))
+                              (tap finger2 roll-right (/ force 10)))))
        (prop-view (list (prop)))))))
 
 #+end_src