1.1 --- a/org/test-creature.org	Sat Jan 07 02:37:36 2012 -0700
     1.2 +++ b/org/test-creature.org	Sat Jan 07 05:06:58 2012 -0700
     1.3 @@ -50,7 +50,7 @@
     1.4  
     1.5  (rlm.rlm-commands/help)
     1.6  
     1.7 -(declare joint-create get-subjective-position)
     1.8 +(declare joint-create)
     1.9  
    1.10  (defn load-bullet []
    1.11    (let [sim (world (Node.) {} no-op no-op)]
    1.12 @@ -117,6 +117,126 @@
    1.13             (take 2 targets))
    1.14            (recur (float (* radius 2))))))))
    1.15  
    1.16 +(defn world-to-local
    1.17 +   "Convert the world coordinates into coordinates relative to the 
    1.18 +   object (i.e. local coordinates), taking into account the rotation
    1.19 +   of object."
    1.20 +   [#^Spatial object world-coordinate]
    1.21 +   (let [out (Vector3f.)]
    1.22 +     (.worldToLocal  object world-coordinate out) out))
    1.23 +
    1.24 +(defmulti zz
    1.25 +  (fn [a b _ _ _ _ _]
    1.26 +   
    1.27 +    (:type a)))
    1.28 +(defmethod zz :p [a b] (println "hi"))
    1.29 +
    1.30 +
    1.31 +(defmulti joint-dispatch
    1.32 +  "Translate blender pseudo-joints into real JME joints."
    1.33 +  (fn [constraints _ _ _ _ _]
    1.34 +    (:type constraints)))
    1.35 +
    1.36 +(defmethod joint-dispatch :point
    1.37 +  [constraints control-a control-b pivot-a pivot-b rotation]
    1.38 +  (println-repl "creating POINT2POINT joint")
    1.39 +  (Point2PointJoint.
    1.40 +   control-a
    1.41 +   control-b
    1.42 +   pivot-a
    1.43 +   pivot-b))
    1.44 +
    1.45 +(defmethod joint-dispatch :hinge
    1.46 +  [constraints control-a control-b pivot-a pivot-b rotation]
    1.47 +  (println-repl "creating HINGE joint")
    1.48 +  (let [axis
    1.49 +        (if-let
    1.50 +            [axis (:axis constraints)]
    1.51 +          axis
    1.52 +          Vector3f/UNIT_X)
    1.53 +        [limit-1 limit-2] (:limit constraints)
    1.54 +        hinge-axis
    1.55 +        (.mult
    1.56 +         rotation
    1.57 +         (blender-to-jme axis))]
    1.58 +    (doto
    1.59 +        (HingeJoint.
    1.60 +         control-a
    1.61 +         control-b
    1.62 +         pivot-a
    1.63 +         pivot-b
    1.64 +         hinge-axis
    1.65 +         hinge-axis)
    1.66 +      (.setLimit limit-1 limit-2))))
    1.67 +
    1.68 +
    1.69 +(defmethod joint-dispatch :cone
    1.70 +  [constraints control-a control-b pivot-a pivot-b rotation]
    1.71 +  (let [limit-xz (:limit-xz constraints)
    1.72 +        limit-xy (:limit-xy constraints)
    1.73 +        twist    (:twist constraints)]
    1.74 +    
    1.75 +    
    1.76 +    (println-repl "creating CONE joint")
    1.77 +    (println-repl rotation)
    1.78 +    (println-repl
    1.79 +     "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
    1.80 +    (println-repl
    1.81 +     "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
    1.82 +    (println-repl
    1.83 +     "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
    1.84 +    (doto
    1.85 +        (ConeJoint.
    1.86 +         control-a
    1.87 +         control-b
    1.88 +         pivot-a
    1.89 +         pivot-b
    1.90 +         rotation
    1.91 +         rotation)
    1.92 +      (.setLimit (float limit-xz)
    1.93 +                 (float limit-xy)
    1.94 +                 (float twist)))))
    1.95 +
    1.96 +
    1.97 +  
    1.98 +(defn connect*
    1.99 +  "here are some examples:
   1.100 +   {:type :point}
   1.101 +   {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
   1.102 +   (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
   1.103 +
   1.104 +   {:type :cone   :limit-xz 0]
   1.105 +                  :limit-xy 0]
   1.106 +                  :twist 0]}   (use XZY rotation mode in blender!)"
   1.107 +  [#^Node obj-a #^Node obj-b #^Node joint]
   1.108 +  (let [control-a (.getControl obj-a RigidBodyControl)
   1.109 +        control-b (.getControl obj-b RigidBodyControl)
   1.110 +        joint-center (.getWorldTranslation joint)
   1.111 +        joint-rotation (.toRotationMatrix (.getWorldRotation joint))
   1.112 +        pivot-a (world-to-local obj-a joint-center)
   1.113 +        pivot-b (world-to-local obj-b joint-center)]
   1.114 +    ;; A side-effect of creating a joint registers
   1.115 +    ;; it with both physics objects which in turn
   1.116 +    ;; will register the joint with the physics system
   1.117 +    ;; when the simulation is started.
   1.118 +    (if-let [constraints
   1.119 +             (map-vals
   1.120 +              eval
   1.121 +              (read-string
   1.122 +               (meta-data joint "joint")))]
   1.123 +      (do
   1.124 +        (println-repl "creating joint between"
   1.125 +                      (.getName obj-a) "and" (.getName obj-b))
   1.126 +        (joint-dispatch constraints
   1.127 +                        control-a control-b
   1.128 +                        pivot-a pivot-b
   1.129 +                        joint-rotation))
   1.130 +                        
   1.131 +      (println-repl "could not find joint meta-data!"))))
   1.132 +
   1.133 +   
   1.134 +
   1.135 +
   1.136  (defn connect
   1.137    "here are some examples:
   1.138     {:type :point}
   1.139 @@ -127,13 +247,12 @@
   1.140                    :limit-xy 0]
   1.141                    :twist 0]}   (use XZY rotation mode in blender!)"
   1.142    [#^Node obj-a #^Node obj-b #^Node joint]
   1.143 -  (let [center-a (.getWorldTranslation obj-a)
   1.144 -        center-b (.getWorldTranslation obj-b)
   1.145 +  (let [control-a (.getControl obj-a RigidBodyControl)
   1.146 +        control-b (.getControl obj-b RigidBodyControl)
   1.147          joint-center (.getWorldTranslation joint)
   1.148 -        pivot-a (.subtract joint-center center-a)
   1.149 -        pivot-b (.subtract joint-center center-b)
   1.150 -        control-a (.getControl obj-a RigidBodyControl)
   1.151 -        control-b (.getControl obj-b RigidBodyControl)]
   1.152 +        joint-rotation (.toRotationMatrix (.getWorldRotation joint))
   1.153 +        pivot-a (world-to-local obj-a joint-center)
   1.154 +        pivot-b (world-to-local obj-b joint-center)]
   1.155      ;; A side-effect of creating a joint registers
   1.156      ;; it with both physics objects which in turn
   1.157      ;; will register the joint with the physics system
   1.158 @@ -180,25 +299,25 @@
   1.159                (do
   1.160                  (let [limit-xz (:limit-xz constraints)
   1.161                        limit-xy (:limit-xy constraints)
   1.162 -                      twist    (:twist constraints)
   1.163 -                      rotation (.toRotationMatrix (.getWorldRotation joint))]
   1.164 +                      twist    (:twist constraints)]
   1.165 +                     
   1.166                    
   1.167                    (println-repl "creating CONE joint")
   1.168 -                  (println-repl rotation)
   1.169 +                  (println-repl joint-rotation)
   1.170                    (println-repl
   1.171 -                   "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
   1.172 +                   "UNIT_X --> " (.mult joint-rotation (Vector3f. 1 0 0)))
   1.173                    (println-repl
   1.174 -                   "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
   1.175 +                   "UNIT_Y --> " (.mult joint-rotation (Vector3f. 0 1 0)))
   1.176                    (println-repl
   1.177 -                   "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
   1.178 +                   "UNIT_Z --> " (.mult joint-rotation (Vector3f. 0 0 1)))
   1.179                    (doto
   1.180                        (ConeJoint.
   1.181                         control-a
   1.182                         control-b
   1.183                         pivot-a
   1.184                         pivot-b
   1.185 -                       rotation
   1.186 -                       rotation
   1.187 +                       joint-rotation
   1.188 +                       joint-rotation
   1.189                         )
   1.190                      (.setLimit (float limit-xz)
   1.191                                 (float limit-xy)
   1.192 @@ -234,7 +353,7 @@
   1.193      (fn [joint]
   1.194        (let [[obj-a obj-b]
   1.195              (joint-targets pieces joint)]
   1.196 -        (connect obj-a obj-b joint)))
   1.197 +        (connect* obj-a obj-b joint)))
   1.198      joints))
   1.199    pieces)
   1.200  
   1.201 @@ -275,7 +394,7 @@
   1.202     (comp light-up-everything enable-debug
   1.203           (fn [world]
   1.204             (.setTimer world (NanoTimer.))
   1.205 -           (set-gravity world (Vector3f. 0 0 0))
   1.206 +           ;;(set-gravity world (Vector3f. 0 0 0))
   1.207             (speed-up world)
   1.208             world
   1.209             ))
   1.210 @@ -324,8 +443,8 @@
   1.211       
   1.212         (doto (ConeJoint.
   1.213                a b
   1.214 -              (get-subjective-position joint-position top)
   1.215 -              (get-subjective-position joint-position bottom)
   1.216 +              (world-to-local top joint-position)
   1.217 +              (world-to-local bottom joint-position)
   1.218                joint-rotation
   1.219                joint-rotation
   1.220                )
   1.221 @@ -405,91 +524,10 @@
   1.222  
   1.223  
   1.224  
   1.225 +#+end_src
   1.226  
   1.227 -
   1.228 -
   1.229 -
   1.230 -
   1.231 -
   1.232 -
   1.233 -
   1.234 -
   1.235 -
   1.236 -;; please validate these.
   1.237 -
   1.238 -(defn get-subjective-position
   1.239 -   "Convert the world coordinates into coordinates relative to
   1.240 -   object (i.e. local coordinates), taking into account the rotation
   1.241 -   of object."
   1.242 -   [#^Vector3f world-coordinates object]
   1.243 -   ;; I don't know if it's important to take into account the rotation
   1.244 -   ;; of the object. If it isn't, then remove the multiplication-by-inverse.
   1.245 -   (.mult (.inverse (.getWorldRotation object))
   1.246 -          (.subtract world-coordinates (.getWorldTranslation object))))
   1.247 -   
   1.248 -
   1.249 -(defn get-subjective-rotation
   1.250 -  "cf get-subjective-position. Converts a rotation specified relative
   1.251 -to the world's axes into a rotation specified in terms of the object's
   1.252 -  coordinate system."
   1.253 -  [#^Quaternion world-rotation object]
   1.254 -  (.mult (.inverse (.getWorldRotation object)) world-rotation))
   1.255 -
   1.256 -
   1.257 -
   1.258 -
   1.259 -(defn joint-create "Connect objects 1 and 2 using a joint constraint. If
   1.260 -  only position is specified, creates a point-to-point joint at the
   1.261 -  given location
   1.262 -  in world coordinates. etc. etc. for other joints.
   1.263 -To ensure consistency, I may alter this function
   1.264 -  so that it moves obj-1 to be at the apex of the cone.
   1.265 -
   1.266 -  NOTE:
   1.267 -  In the usual construction method, you create a joint and, if your
   1.268 -  contraints are consistent, all the objects snap into position and
   1.269 -  orientation around it, otherwise the systen explodes.
   1.270 -
   1.271 -  This construction method assumes that you have in mind a position and
   1.272 -  rotation for the joint, and that you have already put your objects
   1.273 -  at the required distances from that joint so that no snapping needs
   1.274 -  to occur. The radial distances ('pivot lengths') are therefore just set to be
   1.275 -  the pre-existing distances between the objects and the joint." 
   1.276 -  [#^Node obj-1
   1.277 -   #^Node obj-2
   1.278 -   #^Vector3f joint-position
   1.279 -   #^Quaternion joint-rotation
   1.280 -   span-1
   1.281 -   span-2
   1.282 -   twist
   1.283 -   ]
   1.284 -
   1.285 -  (let
   1.286 -      [body-1 (.getControl obj-1 RigidBodyControl)
   1.287 -       body-2 (.getControl obj-2 RigidBodyControl)
   1.288 -       ]
   1.289 -    (doto (ConeJoint.
   1.290 -           body-1
   1.291 -           body-2
   1.292 -           (get-subjective-position joint-position body-1)
   1.293 -           (get-subjective-position joint-position body-2)
   1.294 -           ;; ALIGN X-AXIS OF OBJECT-1 WITH THE CENTRAL AXIS OF THE CONE TO
   1.295 -           ;;LOWER THE RISK OF INCONSISTENCY
   1.296 -           ;;(Matrix3f/IDENTITY)
   1.297 -           (.toRotationMatrix (get-subjective-rotation joint-rotation body-1))
   1.298 -           (.toRotationMatrix (get-subjective-rotation joint-rotation body-2))
   1.299 -           )
   1.300 -      (.setLimit
   1.301 -       span-1
   1.302 -       span-2
   1.303 -       twist))))
   1.304 -
   1.305 -
   1.306 -
   1.307 -
   1.308 -
   1.309 -
   1.310 -#+end_src
   1.311 +#+results: body-1
   1.312 +: #'cortex.silly/test-joint
   1.313  
   1.314  
   1.315  * COMMENT purgatory