1.1 --- a/org/test-creature.org	Sat Jan 07 01:10:14 2012 -0700
     1.2 +++ b/org/test-creature.org	Sat Jan 07 01:32:15 2012 -0700
     1.3 @@ -180,59 +180,29 @@
     1.4                (do
     1.5                  (let [limit-xz (:limit-xz constraints)
     1.6                        limit-xy (:limit-xy constraints)
     1.7 -                      twist    (:twist constraints)]
     1.8 +                      twist    (:twist constraints)
     1.9 +                      rotation (.toRotationMatrix (.getWorldRotation joint))]
    1.10                    
    1.11                    (println-repl "creating CONE joint")
    1.12 -                  (let [frame-a
    1.13 -                        (.toRotationMatrix
    1.14 -                         (doto (Quaternion.)
    1.15 -                           (.fromAngleAxis
    1.16 -                            (float
    1.17 -                             (.angleBetween
    1.18 -                              (.normalize pivot-a) Vector3f/UNIT_X))
    1.19 -                            (.normalize
    1.20 -                             (.cross pivot-a
    1.21 -                                     Vector3f/UNIT_X)))))
    1.22 -                         ]
    1.23 -                    (println-repl "pivot-a" pivot-a)
    1.24 -                    (println-repl
    1.25 -                     "angle between pivot-a and UNIT_X is"
    1.26 -                     (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))
    1.27 -                    (println-repl "frame-a:" frame-a)
    1.28 -                    (println-repl
    1.29 -                     "frame-a moves Vector3f/UNIT_X to"
    1.30 -                     (.mult frame-a Vector3f/UNIT_X ))
    1.31 -                    
    1.32 -                    
    1.33 -                    (doto
    1.34 -                        (ConeJoint.
    1.35 -                         control-a
    1.36 -                         control-b
    1.37 -                         pivot-a
    1.38 -                         pivot-b
    1.39 -
    1.40 -                        
    1.41 -                         ;; frame-in-A
    1.42 -                         ;;frame-a
    1.43 -                         ;;frame-a
    1.44 -                         
    1.45 -                         (.toRotationMatrix
    1.46 -                          (doto (Quaternion.)
    1.47 -                            (.fromAngles
    1.48 -                             0 0 (* -0.5 (/ Math/PI 2)))))
    1.49 -                         
    1.50 -                         
    1.51 -                         ;; frame-in-B
    1.52 -                         (.toRotationMatrix
    1.53 -                          (doto (Quaternion.)
    1.54 -                            (.fromAngles
    1.55 -                             0 0 (* -0.5 (/ Math/PI 2)))))
    1.56 -                         
    1.57 -
    1.58 -                         )
    1.59 -                      (.setLimit (float limit-xz)
    1.60 -                                 (float limit-xy)
    1.61 -                                 (float twist))))))
    1.62 +                  (println-repl rotation)
    1.63 +                  (println-repl
    1.64 +                   "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
    1.65 +                  (println-repl
    1.66 +                   "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
    1.67 +                  (println-repl
    1.68 +                   "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
    1.69 +                  (doto
    1.70 +                      (ConeJoint.
    1.71 +                       control-a
    1.72 +                       control-b
    1.73 +                       pivot-a
    1.74 +                       pivot-b
    1.75 +                       rotation
    1.76 +                       rotation
    1.77 +                       )
    1.78 +                    (.setLimit (float limit-xz)
    1.79 +                               (float limit-xy)
    1.80 +                               (float twist)))))
    1.81                true
    1.82                (println-repl
    1.83                 "joint-type" joint-type "not recognized")))
    1.84 @@ -321,16 +291,11 @@
    1.85            (doto (Quaternion.)
    1.86              (.fromAngleAxis
    1.87               (* 1 (/ Math/PI 4))
    1.88 -             (Vector3f. 0 0 1)))
    1.89 -          (.mult
    1.90 -           (doto (Quaternion.)
    1.91 -             (.fromAngleAxis
    1.92 -              (* 2 (/ Math/PI 4))
    1.93 -              (Vector3f. 0 1 0)))
    1.94 -           (doto (Quaternion.)
    1.95 -             (.fromAngleAxis
    1.96 -              (* 1 (/ Math/PI 2))
    1.97 -              (Vector3f. 0 0 1))))))
    1.98 +             (Vector3f. -1 0 0)))
    1.99 +          (doto (Quaternion.)
   1.100 +            (.fromAngleAxis
   1.101 +             (* 1 (/ Math/PI 2))
   1.102 +             (Vector3f. 0 0 1)))))
   1.103          top-position (.mult joint-rotation (Vector3f. 8 0 0))
   1.104          
   1.105          origin (doto