1.1 --- a/org/test-creature.org	Fri Jan 06 02:03:43 2012 -0700
     1.2 +++ b/org/test-creature.org	Sat Jan 07 00:58:47 2012 -0700
     1.3 @@ -50,6 +50,11 @@
     1.4  
     1.5  (rlm.rlm-commands/help)
     1.6  
     1.7 +(declare joint-create get-subjective-position)
     1.8 +
     1.9 +(defn load-bullet []
    1.10 +  (.start (world (Node.) {} no-op no-op)))
    1.11 +
    1.12  (defn load-blender-model
    1.13    "Load a .blend file using an asset folder relative path."
    1.14    [^String model]
    1.15 @@ -179,14 +184,19 @@
    1.16                             (.fromAngleAxis
    1.17                              (float
    1.18                               (.angleBetween
    1.19 -                              pivot-a Vector3f/UNIT_X))
    1.20 -                            (.cross (.normalize pivot-a)
    1.21 -                                    Vector3f/UNIT_X))))
    1.22 -                        ]
    1.23 +                              (.normalize pivot-a) Vector3f/UNIT_X))
    1.24 +                            (.normalize
    1.25 +                             (.cross pivot-a
    1.26 +                                     Vector3f/UNIT_X)))))
    1.27 +                         ]
    1.28 +                    (println-repl "pivot-a" pivot-a)
    1.29                      (println-repl
    1.30 -                     "angle between pivot-a (" pivot-a ") and UNIT_X is"
    1.31 +                     "angle between pivot-a and UNIT_X is"
    1.32                       (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))
    1.33 -                    
    1.34 +                    (println-repl "frame-a:" frame-a)
    1.35 +                    (println-repl
    1.36 +                     "frame-a moves Vector3f/UNIT_X to"
    1.37 +                     (.mult frame-a Vector3f/UNIT_X ))
    1.38                      
    1.39                      
    1.40                      (doto
    1.41 @@ -197,22 +207,22 @@
    1.42                           pivot-b
    1.43  
    1.44                          
    1.45 -                         ;; ;; frame-in-A
    1.46 -                         frame-a
    1.47 -                         frame-a
    1.48 +                         ;; frame-in-A
    1.49 +                         ;;frame-a
    1.50 +                         ;;frame-a
    1.51                           
    1.52 -                         ;; (.toRotationMatrix
    1.53 -                         ;;  (doto (Quaternion.)
    1.54 -                         ;;    (.fromAngles
    1.55 -                         ;;     0 0 (* -1 (/ Math/PI 2)))))
    1.56 -
    1.57 +                         (.toRotationMatrix
    1.58 +                          (doto (Quaternion.)
    1.59 +                            (.fromAngles
    1.60 +                             0 0 (* -0.5 (/ Math/PI 2)))))
    1.61                           
    1.62 -                         ;; ;; frame-in-B
    1.63 -                         ;; (.toRotationMatrix
    1.64 -                         ;;  (doto (Quaternion.)
    1.65 -                         ;;    (.fromAngles
    1.66 -                         ;;     0 0 (* -1.2 (/ Math/PI 2)))))
    1.67 -
    1.68 +                         
    1.69 +                         ;; frame-in-B
    1.70 +                         (.toRotationMatrix
    1.71 +                          (doto (Quaternion.)
    1.72 +                            (.fromAngles
    1.73 +                             0 0 (* -0.5 (/ Math/PI 2)))))
    1.74 +                         
    1.75  
    1.76                           )
    1.77                        (.setLimit (float limit-xz)
    1.78 @@ -224,6 +234,7 @@
    1.79        
    1.80        (println-repl "could not find joint meta-data!"))))
    1.81  
    1.82 +
    1.83  (defn assemble-creature [#^Node pieces joints]
    1.84    (dorun
    1.85     (map
    1.86 @@ -296,105 +307,63 @@
    1.87     no-op)))
    1.88  
    1.89  (defn world-setup [joint]
    1.90 -  (let [top (doto
    1.91 +  (let [
    1.92 +
    1.93 +        joint-position (Vector3f. 0 4 0)
    1.94 +        joint-rotation
    1.95 +        (.toRotationMatrix
    1.96 +         (.mult
    1.97 +          (doto (Quaternion.)
    1.98 +            (.fromAngleAxis
    1.99 +             (* 1 (/ Math/PI 4))
   1.100 +             (Vector3f. -1 0 0)))
   1.101 +          (doto (Quaternion.)
   1.102 +            (.fromAngleAxis
   1.103 +             (/ Math/PI 2)
   1.104 +             (Vector3f. 0 0 1)))))
   1.105 +        
   1.106 +        origin (doto
   1.107 +                   (sphere 0.1 :physical? false :color ColorRGBA/Cyan
   1.108 +                           :position (Vector3f. 0 0 0)))
   1.109 +        top (doto
   1.110                  (sphere 0.1 :physical? false :color ColorRGBA/Yellow
   1.111 -                        :position (Vector3f. 0 7 0))
   1.112 +                        :position (.mult joint-rotation (Vector3f. 8 0 0)))
   1.113 +
   1.114                (.addControl
   1.115                 (RigidBodyControl.
   1.116 -                (CapsuleCollisionShape. 0.5 1.5 1) (float 15))))
   1.117 +                (CapsuleCollisionShape. 0.5 1.5 1) (float 20))))
   1.118          bottom (doto
   1.119                     (sphere 0.1 :physical? false :color ColorRGBA/DarkGray
   1.120 -                           :position (Vector3f. 0 -1 0))
   1.121 -              (.addControl
   1.122 +                           :position (Vector3f. 0 0 0))
   1.123 +                 (.addControl
   1.124                 (RigidBodyControl.
   1.125                  (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))
   1.126 -        table (box 10 2 10 :position (Vector3f. 0 -6 0)
   1.127 +        table (box 10 2 10 :position (Vector3f. 0 -20 0)
   1.128                     :color ColorRGBA/Gray :mass 0)
   1.129          a (.getControl top RigidBodyControl)
   1.130          b (.getControl bottom RigidBodyControl)]
   1.131 +
   1.132      (cond
   1.133 -     (= joint :point)
   1.134 -     (doto
   1.135 -         (Point2PointJoint. a b
   1.136 -                            (Vector3f. 0 -2 0)
   1.137 -                            (Vector3f. 0  2 0))
   1.138 -       (.setCollisionBetweenLinkedBodys false))
   1.139 -     (= joint :hinge)
   1.140 -     (doto
   1.141 -         (HingeJoint.
   1.142 -          a b
   1.143 -          (Vector3f. 0 -2 0)
   1.144 -          (Vector3f. 0 2 0)
   1.145 -          (Vector3f. 0 0 1)
   1.146 -          (Vector3f. 0 0 1)
   1.147 +     (= joint :cone)
   1.148 +     
   1.149 +       (doto (ConeJoint.
   1.150 +              a b
   1.151 +              (get-subjective-position joint-position top)
   1.152 +              (get-subjective-position joint-position bottom)
   1.153 +              joint-rotation
   1.154 +              joint-rotation
   1.155 +              )
   1.156  
   1.157 -          )
   1.158 -       (.setCollisionBetweenLinkedBodys false)
   1.159 -       ;;(.setLimit (- Math/PI) Math/PI)
   1.160 -       )
   1.161 -     (= joint :cone)
   1.162 -     ;; note to self -- jbullet does NOT implement cone joints
   1.163 -     ;; correctly. You must use plain ol' bullet for this to work.
   1.164 -     ;; It's faster anyway, so whatever.
   1.165 -     
   1.166 -     (doto (ConeJoint.
   1.167 -            a b
   1.168 -            (Vector3f. 0 -5 0)
   1.169 -            (Vector3f. 0 2 0)
   1.170 -
   1.171 -            (doto (Matrix3f.)
   1.172 -              (.fromStartEndVectors Vector3f/UNIT_X
   1.173 -                                    Vector3f/UNIT_Y))
   1.174 -            (doto (Matrix3f.)
   1.175 -              (.fromStartEndVectors Vector3f/UNIT_X
   1.176 -                                    (.normalize
   1.177 -                                     (Vector3f. 0 0 -1))))
   1.178 -            )
   1.179 -       ;;(.setAngularOnly true)
   1.180 -       
   1.181 -       (.setCollisionBetweenLinkedBodys false)
   1.182 -       (.setLimit (* 1 (/ Math/PI 4))
   1.183 -                  (* 1 (/ Math/PI 2))
   1.184 -                  (* 0 (/ Math/PI 4)))
   1.185 -        
   1.186 -       )
   1.187 -     (= joint :six)
   1.188 -     (doto
   1.189 -         (SixDofJoint.
   1.190 -          a b
   1.191 -          (Vector3f. 0 -2 0)
   1.192 -          (Vector3f. 0 2 0)
   1.193 -          ;;(doto (Matrix3f.)
   1.194 -          ;;    (.fromStartEndVectors Vector3f/UNIT_X
   1.195 -          ;;                          Vector3f/UNIT_Y))
   1.196 -          ;;(doto (Matrix3f.)
   1.197 -          ;;  (.fromStartEndVectors Vector3f/UNIT_X
   1.198 -          ;;                        Vector3f/UNIT_Y))
   1.199 -          true)
   1.200 -       (.setCollisionBetweenLinkedBodys false)
   1.201 -       (.setAngularLowerLimit (Vector3f. 0
   1.202 -                                         (- (/ Math/PI 2))
   1.203 -                                         0))
   1.204 -                                         
   1.205 -       (.setAngularUpperLimit (Vector3f. 0
   1.206 -                                         (/ Math/PI 2)
   1.207 -                                         0))
   1.208 -       (.setLinearLowerLimit  (Vector3f. 0 0 0))
   1.209 -       (.setLinearUpperLimit  (Vector3f. 0 0 0))
   1.210 -
   1.211 -       )
   1.212 -          
   1.213 +              
   1.214 +         (.setLimit (* (/ 10) Math/PI)
   1.215 +                    (* (/ 4)  Math/PI)
   1.216 +                    0)))
   1.217 +  [origin top bottom table]))
   1.218              
   1.219 -            
   1.220 -          
   1.221 -     
   1.222 -    )
   1.223 -      
   1.224 -    [top bottom table]))
   1.225  
   1.226  
   1.227  (defn test-joint [joint]
   1.228 -  (let [[top bottom floor] (world-setup joint)
   1.229 +  (let [[origin top bottom floor] (world-setup joint)
   1.230          control (.getControl top RigidBodyControl)
   1.231          move-up? (atom false)
   1.232          move-down? (atom false)
   1.233 @@ -405,7 +374,7 @@
   1.234          timer (atom 0)]
   1.235  
   1.236      (world
   1.237 -     (nodify [top bottom floor])
   1.238 +     (nodify [top bottom floor origin])
   1.239       (merge standard-debug-controls
   1.240              {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.241               "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.242 @@ -427,7 +396,12 @@
   1.243             (.attachChild (.getRootNode world)
   1.244                         (sphere 0.05 :color ColorRGBA/Yellow
   1.245                                 :position (.getWorldTranslation top)
   1.246 -                               :physical? false))))
   1.247 +                               :physical? false))
   1.248 +           (.attachChild (.getRootNode world)
   1.249 +                         (sphere 0.05 :color ColorRGBA/LightGray
   1.250 +                                 :position (.getWorldTranslation bottom)
   1.251 +                                 :physical? false))))
   1.252 +
   1.253         (if @move-up?
   1.254           (.applyTorque control
   1.255                         (.mult (.getPhysicsRotation control)
   1.256 @@ -452,6 +426,93 @@
   1.257           (.applyTorque control
   1.258                         (.mult (.getPhysicsRotation control)
   1.259                                (Vector3f. 1 0 0))))))))
   1.260 +
   1.261 +
   1.262 +
   1.263 +
   1.264 +
   1.265 +
   1.266 +
   1.267 +
   1.268 +
   1.269 +
   1.270 +
   1.271 +
   1.272 +
   1.273 +;; please validate these.
   1.274 +
   1.275 +(defn get-subjective-position
   1.276 +   "Convert the world coordinates into coordinates relative to
   1.277 +   object (i.e. local coordinates), taking into account the rotation
   1.278 +   of object."
   1.279 +   [#^Vector3f world-coordinates object]
   1.280 +   ;; I don't know if it's important to take into account the rotation
   1.281 +   ;; of the object. If it isn't, then remove the multiplication-by-inverse.
   1.282 +   (.mult (.inverse (.getWorldRotation object))
   1.283 +          (.subtract world-coordinates (.getWorldTranslation object))))
   1.284 +   
   1.285 +
   1.286 +(defn get-subjective-rotation
   1.287 +  "cf get-subjective-position. Converts a rotation specified relative
   1.288 +to the world's axes into a rotation specified in terms of the object's
   1.289 +  coordinate system."
   1.290 +  [#^Quaternion world-rotation object]
   1.291 +  (.mult (.inverse (.getWorldRotation object)) world-rotation))
   1.292 +
   1.293 +
   1.294 +
   1.295 +
   1.296 +(defn joint-create "Connect objects 1 and 2 using a joint constraint. If
   1.297 +  only position is specified, creates a point-to-point joint at the
   1.298 +  given location
   1.299 +  in world coordinates. etc. etc. for other joints.
   1.300 +To ensure consistency, I may alter this function
   1.301 +  so that it moves obj-1 to be at the apex of the cone.
   1.302 +
   1.303 +  NOTE:
   1.304 +  In the usual construction method, you create a joint and, if your
   1.305 +  contraints are consistent, all the objects snap into position and
   1.306 +  orientation around it, otherwise the systen explodes.
   1.307 +
   1.308 +  This construction method assumes that you have in mind a position and
   1.309 +  rotation for the joint, and that you have already put your objects
   1.310 +  at the required distances from that joint so that no snapping needs
   1.311 +  to occur. The radial distances ('pivot lengths') are therefore just set to be
   1.312 +  the pre-existing distances between the objects and the joint." 
   1.313 +  [#^Node obj-1
   1.314 +   #^Node obj-2
   1.315 +   #^Vector3f joint-position
   1.316 +   #^Quaternion joint-rotation
   1.317 +   span-1
   1.318 +   span-2
   1.319 +   twist
   1.320 +   ]
   1.321 +
   1.322 +  (let
   1.323 +      [body-1 (.getControl obj-1 RigidBodyControl)
   1.324 +       body-2 (.getControl obj-2 RigidBodyControl)
   1.325 +       ]
   1.326 +    (doto (ConeJoint.
   1.327 +           body-1
   1.328 +           body-2
   1.329 +           (get-subjective-position joint-position body-1)
   1.330 +           (get-subjective-position joint-position body-2)
   1.331 +           ;; ALIGN X-AXIS OF OBJECT-1 WITH THE CENTRAL AXIS OF THE CONE TO
   1.332 +           ;;LOWER THE RISK OF INCONSISTENCY
   1.333 +           ;;(Matrix3f/IDENTITY)
   1.334 +           (.toRotationMatrix (get-subjective-rotation joint-rotation body-1))
   1.335 +           (.toRotationMatrix (get-subjective-rotation joint-rotation body-2))
   1.336 +           )
   1.337 +      (.setLimit
   1.338 +       span-1
   1.339 +       span-2
   1.340 +       twist))))
   1.341 +
   1.342 +
   1.343 +
   1.344 +
   1.345 +
   1.346 +
   1.347  #+end_src
   1.348  
   1.349