Mercurial > cortex
diff org/test-creature.org @ 77:1f84f425e05d
first draft of automatic constraints from blender
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Wed, 04 Jan 2012 08:36:42 -0700 |
parents | f4c77512808e |
children | 77b506ac64f3 |
line wrap: on
line diff
1.1 --- a/org/test-creature.org Wed Dec 28 23:12:32 2011 -0700 1.2 +++ b/org/test-creature.org Wed Jan 04 08:36:42 2012 -0700 1.3 @@ -66,8 +66,6 @@ 1.4 (.findValue data key) 1.5 nil)) 1.6 1.7 -(defn hand2 [] 1.8 - (load-blender-model "Models/creature1/try-again.blend")) 1.9 1.10 (defn hand [] 1.11 (load-blender-model "Models/creature1/one.blend")) 1.12 @@ -100,55 +98,450 @@ 1.13 (map #(.getWorldTranslation %) 1.14 (filter #(re-matches #"joint\.\d\d\d" (.getName %)) 1.15 (node-seq (hand))))) 1.16 +(defn worm [] 1.17 + (load-blender-model "Models/creature1/try-again.blend")) 1.18 + 1.19 1.20 (defn worm-pieces [] 1.21 (filter 1.22 (comp #{"worm-2" "worm-1"} 1.23 #(apply str (drop-last (.getName %)))) 1.24 - (node-seq (hand2)))) 1.25 + (node-seq (worm)))) 1.26 1.27 (defn worm-joints [] 1.28 - [Vector3f/ZERO]) 1.29 + (filter #(re-matches #"joint\.\d\d\d" (.getName %)) 1.30 + (node-seq (worm)))) 1.31 1.32 +(defn bullet-trans [] 1.33 + (let [obj-a (sphere 0.5 :color ColorRGBA/Red 1.34 + :position (Vector3f. -10 5 0)) 1.35 + obj-b (sphere 0.5 :color ColorRGBA/Blue 1.36 + :position (Vector3f. -10 -5 0) 1.37 + :mass 0) 1.38 + control-a (.getControl obj-a RigidBodyControl) 1.39 + control-b (.getControl obj-b RigidBodyControl) 1.40 + swivel 1.41 + (.toRotationMatrix 1.42 + (doto (Quaternion.) 1.43 + (.fromAngleAxis (/ Math/PI 2) 1.44 + Vector3f/UNIT_X)))] 1.45 + (doto 1.46 + (ConeJoint. 1.47 + control-a control-b 1.48 + (Vector3f. 0 5 0) 1.49 + (Vector3f. 0 -5 0) 1.50 + swivel swivel) 1.51 + (.setLimit (* 0.6 (/ Math/PI 4)) 1.52 + (/ Math/PI 4) 1.53 + (* Math/PI 0.8))) 1.54 + (world (nodify 1.55 + [obj-a obj-b]) 1.56 + standard-debug-controls 1.57 + enable-debug 1.58 + no-op))) 1.59 1.60 1.61 -(defn find-joint 1.62 - [#^Node parts #^Vector3f joint-position] 1.63 +(defn bullet-trans* [] 1.64 + (let [obj-a (box 1.5 0.5 0.5 :color ColorRGBA/Red 1.65 + :position (Vector3f. 5 0 0) 1.66 + :mass 90) 1.67 + obj-b (sphere 0.5 :color ColorRGBA/Blue 1.68 + :position (Vector3f. -5 0 0) 1.69 + :mass 0) 1.70 + control-a (.getControl obj-a RigidBodyControl) 1.71 + control-b (.getControl obj-b RigidBodyControl) 1.72 + move-up? (atom nil) 1.73 + move-down? (atom nil) 1.74 + move-left? (atom nil) 1.75 + move-right? (atom nil) 1.76 + roll-left? (atom nil) 1.77 + roll-right? (atom nil) 1.78 + force 100 1.79 + swivel 1.80 + (.toRotationMatrix 1.81 + (doto (Quaternion.) 1.82 + (.fromAngleAxis (/ Math/PI 2) 1.83 + Vector3f/UNIT_X))) 1.84 + x-move 1.85 + (doto (Matrix3f.) 1.86 + (.fromStartEndVectors Vector3f/UNIT_X 1.87 + (.normalize (Vector3f. 1 1 0)))) 1.88 + 1.89 + timer (atom 0)] 1.90 + (doto 1.91 + (ConeJoint. 1.92 + control-a control-b 1.93 + (Vector3f. -8 0 0) 1.94 + (Vector3f. 2 0 0) 1.95 + ;;swivel swivel 1.96 + ;;Matrix3f/IDENTITY Matrix3f/IDENTITY 1.97 + x-move Matrix3f/IDENTITY 1.98 + ) 1.99 + (.setCollisionBetweenLinkedBodys false) 1.100 + (.setLimit (* 1 (/ Math/PI 4)) ;; twist 1.101 + (* 1 (/ Math/PI 4)) ;; swing span in X-Y plane 1.102 + (* 0 (/ Math/PI 4)))) ;; swing span in Y-Z plane 1.103 + (world (nodify 1.104 + [obj-a obj-b]) 1.105 + (merge standard-debug-controls 1.106 + {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 1.107 + "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 1.108 + "key-f" (fn [_ pressed?] (reset! move-left? pressed?)) 1.109 + "key-g" (fn [_ pressed?] (reset! move-right? pressed?)) 1.110 + "key-v" (fn [_ pressed?] (reset! roll-left? pressed?)) 1.111 + "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))}) 1.112 + 1.113 + (fn [world] 1.114 + (enable-debug world) 1.115 + (set-gravity world Vector3f/ZERO) 1.116 + ) 1.117 + 1.118 + (fn [world _] 1.119 + 1.120 + (if @move-up? 1.121 + (.applyForce control-a 1.122 + (Vector3f. force 0 0) 1.123 + (Vector3f. 0 0 0))) 1.124 + (if @move-down? 1.125 + (.applyForce control-a 1.126 + (Vector3f. (- force) 0 0) 1.127 + (Vector3f. 0 0 0))) 1.128 + (if @move-left? 1.129 + (.applyForce control-a 1.130 + (Vector3f. 0 force 0) 1.131 + (Vector3f. 0 0 0))) 1.132 + (if @move-right? 1.133 + (.applyForce control-a 1.134 + (Vector3f. 0 (- force) 0) 1.135 + (Vector3f. 0 0 0))) 1.136 + 1.137 + (if @roll-left? 1.138 + (.applyForce control-a 1.139 + (Vector3f. 0 0 force) 1.140 + (Vector3f. 0 0 0))) 1.141 + (if @roll-right? 1.142 + (.applyForce control-a 1.143 + (Vector3f. 0 0 (- force)) 1.144 + (Vector3f. 0 0 0))) 1.145 + 1.146 + (if (zero? (rem (swap! timer inc) 100)) 1.147 + (.attachChild 1.148 + (.getRootNode world) 1.149 + (sphere 0.05 :color ColorRGBA/Yellow 1.150 + :physical? false :position 1.151 + (.getWorldTranslation obj-a))))) 1.152 + ) 1.153 + )) 1.154 + 1.155 + 1.156 + 1.157 + 1.158 + 1.159 + 1.160 + 1.161 + 1.162 + 1.163 + 1.164 + 1.165 +(defn world-setup [joint] 1.166 + (let [top (doto 1.167 + (sphere 0.1 :physical? false :color ColorRGBA/Yellow 1.168 + :position (Vector3f. 0 7 0)) 1.169 + (.addControl 1.170 + (RigidBodyControl. 1.171 + (CapsuleCollisionShape. 0.5 1.5 1) (float 15)))) 1.172 + bottom (doto 1.173 + (sphere 0.1 :physical? false :color ColorRGBA/DarkGray 1.174 + :position (Vector3f. 0 -1 0)) 1.175 + (.addControl 1.176 + (RigidBodyControl. 1.177 + (CapsuleCollisionShape. 0.5 1.5 1) (float 0)))) 1.178 + table (box 10 2 10 :position (Vector3f. 0 -6 0) 1.179 + :color ColorRGBA/Gray :mass 0) 1.180 + a (.getControl top RigidBodyControl) 1.181 + b (.getControl bottom RigidBodyControl)] 1.182 + (cond 1.183 + (= joint :point) 1.184 + (doto 1.185 + (Point2PointJoint. a b 1.186 + (Vector3f. 0 -2 0) 1.187 + (Vector3f. 0 2 0)) 1.188 + (.setCollisionBetweenLinkedBodys false)) 1.189 + (= joint :hinge) 1.190 + (doto 1.191 + (HingeJoint. 1.192 + a b 1.193 + (Vector3f. 0 -2 0) 1.194 + (Vector3f. 0 2 0) 1.195 + (Vector3f. 0 0 1) 1.196 + (Vector3f. 0 0 1) 1.197 + 1.198 + ) 1.199 + (.setCollisionBetweenLinkedBodys false) 1.200 + ;;(.setLimit (- Math/PI) Math/PI) 1.201 + ) 1.202 + (= joint :cone) 1.203 + ;; note to self -- jbullet does NOT implement cone joints 1.204 + ;; correctly. You must use plain ol' bullet for this to work. 1.205 + ;; It's faster anyway, so whatever. 1.206 + 1.207 + (doto (ConeJoint. 1.208 + a b 1.209 + (Vector3f. 0 -5 0) 1.210 + (Vector3f. 0 2 0) 1.211 + 1.212 + (doto (Matrix3f.) 1.213 + (.fromStartEndVectors Vector3f/UNIT_X 1.214 + Vector3f/UNIT_Y)) 1.215 + (doto (Matrix3f.) 1.216 + (.fromStartEndVectors Vector3f/UNIT_X 1.217 + Vector3f/UNIT_Y)) 1.218 + ) 1.219 + ;;(.setAngularOnly true) 1.220 + 1.221 + (.setCollisionBetweenLinkedBodys false) 1.222 + (.setLimit (* 1 (/ Math/PI 4)) 1.223 + (* 1 (/ Math/PI 4)) 1.224 + (* 0 (/ Math/PI 4))) 1.225 + 1.226 + ) 1.227 + (= joint :six) 1.228 + (doto 1.229 + (SixDofJoint. 1.230 + a b 1.231 + (Vector3f. 0 -2 0) 1.232 + (Vector3f. 0 2 0) 1.233 + ;;(doto (Matrix3f.) 1.234 + ;; (.fromStartEndVectors Vector3f/UNIT_X 1.235 + ;; Vector3f/UNIT_Y)) 1.236 + ;;(doto (Matrix3f.) 1.237 + ;; (.fromStartEndVectors Vector3f/UNIT_X 1.238 + ;; Vector3f/UNIT_Y)) 1.239 + true) 1.240 + (.setCollisionBetweenLinkedBodys false) 1.241 + (.setAngularLowerLimit (Vector3f. 0 1.242 + (- (/ Math/PI 2)) 1.243 + 0)) 1.244 + 1.245 + (.setAngularUpperLimit (Vector3f. 0 1.246 + (/ Math/PI 2) 1.247 + 0)) 1.248 + (.setLinearLowerLimit (Vector3f. 0 0 0)) 1.249 + (.setLinearUpperLimit (Vector3f. 0 0 0)) 1.250 + 1.251 + ) 1.252 + 1.253 + 1.254 + 1.255 + 1.256 + 1.257 + ) 1.258 + 1.259 + [top bottom table])) 1.260 + 1.261 +(defn speed-up [world] 1.262 + (.setMoveSpeed (.getFlyByCamera world) 1.263 + (float 100)) 1.264 + (.setRotationSpeed (.getFlyByCamera world) 1.265 + (float 20)) 1.266 + world) 1.267 + 1.268 + 1.269 +(defn test-joint [joint] 1.270 + (let [[top bottom floor] (world-setup joint) 1.271 + control (.getControl top RigidBodyControl) 1.272 + move-up? (atom false) 1.273 + move-down? (atom false) 1.274 + move-left? (atom false) 1.275 + move-right? (atom false) 1.276 + roll-left? (atom false) 1.277 + roll-right? (atom false) 1.278 + timer (atom 0)] 1.279 + 1.280 + (world 1.281 + (nodify [top bottom floor]) 1.282 + (merge standard-debug-controls 1.283 + {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 1.284 + "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 1.285 + "key-f" (fn [_ pressed?] (reset! move-left? pressed?)) 1.286 + "key-g" (fn [_ pressed?] (reset! move-right? pressed?)) 1.287 + "key-v" (fn [_ pressed?] (reset! roll-left? pressed?)) 1.288 + "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))}) 1.289 + 1.290 + (fn [world] 1.291 + (light-up-everything world) 1.292 + (enable-debug world) 1.293 + (set-gravity world (Vector3f. 0 0 0)) 1.294 + ) 1.295 + 1.296 + (fn [world _] 1.297 + (if (zero? (rem (swap! timer inc) 100)) 1.298 + (do 1.299 + ;; (println-repl @timer) 1.300 + (.attachChild (.getRootNode world) 1.301 + (sphere 0.05 :color ColorRGBA/Yellow 1.302 + :position (.getWorldTranslation top) 1.303 + :physical? false)))) 1.304 + (if @move-up? 1.305 + (.applyTorque control 1.306 + (.mult (.getPhysicsRotation control) 1.307 + (Vector3f. 0 0 10)))) 1.308 + (if @move-down? 1.309 + (.applyTorque control 1.310 + (.mult (.getPhysicsRotation control) 1.311 + (Vector3f. 0 0 -10)))) 1.312 + (if @move-left? 1.313 + (.applyTorque control 1.314 + (.mult (.getPhysicsRotation control) 1.315 + (Vector3f. 0 10 0)))) 1.316 + (if @move-right? 1.317 + (.applyTorque control 1.318 + (.mult (.getPhysicsRotation control) 1.319 + (Vector3f. 0 -10 0)))) 1.320 + (if @roll-left? 1.321 + (.applyTorque control 1.322 + (.mult (.getPhysicsRotation control) 1.323 + (Vector3f. -1 0 0)))) 1.324 + (if @roll-right? 1.325 + (.applyTorque control 1.326 + (.mult (.getPhysicsRotation control) 1.327 + (Vector3f. 1 0 0)))))))) 1.328 + 1.329 + 1.330 + 1.331 +(defn run [joint] (.start (test-joint joint))) 1.332 +(defn look [joint] (view (nodify (world-setup joint)))) 1.333 + 1.334 +(defn blender-to-jme 1.335 + "Convert from Blender coordinates to JME coordinates" 1.336 + [#^Vector3f in] 1.337 + (Vector3f. (.getX in) 1.338 + (.getZ in) 1.339 + (- (.getY in)))) 1.340 + 1.341 + 1.342 +(defn joint-targets 1.343 + "Return the two closest two objects to the joint object, ordered 1.344 + from bottom to top according to the joint's rotation." 1.345 + [#^Node parts #^Node joint] 1.346 + ;;(println (meta-data joint "joint")) 1.347 + (.getWorldRotation joint) 1.348 (loop [radius (float 0.01)] 1.349 (let [results (CollisionResults.)] 1.350 (.collideWith 1.351 parts 1.352 - (BoundingBox. joint-position radius radius radius) 1.353 + (BoundingBox. (.getWorldTranslation joint) 1.354 + radius radius radius) 1.355 results) 1.356 (let [targets 1.357 (distinct 1.358 (map #(.getGeometry %) results))] 1.359 (if (>= (count targets) 2) 1.360 - (take 2 targets) 1.361 + (sort-by 1.362 + #(.getY 1.363 + (.mult 1.364 + (.inverse (.getWorldRotation joint)) 1.365 + (.subtract (.getWorldTranslation %) 1.366 + (.getWorldTranslation joint)))) 1.367 + (take 2 targets)) 1.368 (recur (float (* radius 2)))))))) 1.369 1.370 +(defn connect 1.371 + "here are some examples: 1.372 + {:type :point} 1.373 + {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)} 1.374 + (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints) 1.375 1.376 + {:type :cone :limit-xz 0] 1.377 + :limit-yz 0] 1.378 + :twist 0]} 1.379 +" 1.380 + ([#^Node obj-a #^Node obj-b #^Node joint] 1.381 + (let [center-a (.getWorldTranslation obj-a) 1.382 + center-b (.getWorldTranslation obj-b) 1.383 + joint-center (.getWorldTranslation joint) 1.384 + pivot-a (.subtract joint-center center-a) 1.385 + pivot-b (.subtract joint-center center-b) 1.386 + control-a (.getControl obj-a RigidBodyControl) 1.387 + control-b (.getControl obj-b RigidBodyControl)] 1.388 + ;; A side-effect of creating a joint registers 1.389 + ;; it with both physics objects which in turn 1.390 + ;; will register the joint with the physics system 1.391 + ;; when the simulation is started. 1.392 + (if-let [constraints 1.393 + (map-vals 1.394 + eval 1.395 + (read-string 1.396 + (meta-data (first (worm-joints)) "joint")))] 1.397 1.398 -(defn connect-at-point 1.399 - [obj-a obj-b point] 1.400 - (let [center-a (.getWorldTranslation obj-a) 1.401 - center-b (.getWorldTranslation obj-b) 1.402 - pivot-a (.subtract point center-a) 1.403 - pivot-b (.subtract point center-b) 1.404 - ;; A side-effect of creating a joint registers 1.405 - ;; it with both physics objects which in turn 1.406 - ;; will register the joint with the physics system 1.407 - ;; when the simulation is started. 1.408 - joint (Point2PointJoint. 1.409 - (.getControl obj-a RigidBodyControl) 1.410 - (.getControl obj-b RigidBodyControl) 1.411 - pivot-a 1.412 - pivot-b)] 1.413 - obj-a)) 1.414 + (let [joint-type (:type constraints)] 1.415 + (cond (= :point joint-type) 1.416 + (do 1.417 + (println-repl "creating POINT joint") 1.418 + (Point2PointJoint. 1.419 + control-a 1.420 + control-b 1.421 + pivot-a 1.422 + pivot-b)) 1.423 + (= :hinge joint-type) 1.424 + (do 1.425 + (println-repl "creating HINGE joint") 1.426 + (let [axis (if-let 1.427 + [axis (:axis constraints)] 1.428 + axis 1.429 + Vector3f/UNIT_X) 1.430 + [limit-1 limit-2] (:limit constraints) 1.431 + hinge-axis 1.432 + (.mult 1.433 + (.getWorldRotation joint) 1.434 + (blender-to-jme axis))] 1.435 + (doto 1.436 + (HingeJoint. 1.437 + control-a 1.438 + control-b 1.439 + pivot-a 1.440 + pivot-b 1.441 + hinge-axis 1.442 + hinge-axis) 1.443 + (.setLimit limit-1 limit-2)))) 1.444 + (= :cone joint-type) 1.445 + (do 1.446 + (let [limit-xy (:limit-xz constraints) 1.447 + limit-yz (:limit-yz constraints) 1.448 + twist (:twist constraints)] 1.449 + 1.450 + (println-repl "creating CONE joint") 1.451 + (doto 1.452 + (ConeJoint. 1.453 + control-a 1.454 + control-b 1.455 + pivot-a 1.456 + pivot-b 1.457 + (doto (Matrix3f.) 1.458 + (.fromStartEndVectors 1.459 + Vector3f/UNIT_X 1.460 + (.normalize 1.461 + (.subtract (.getWorldTranslation joint) 1.462 + (.getWorldTranslation obj-a))))) 1.463 + (doto (Matrix3f.) 1.464 + (.fromStartEndVectors 1.465 + Vector3f/UNIT_X 1.466 + (.normalize 1.467 + (.subtract 1.468 + (.getWorldTranslation obj-b) 1.469 + (.getWorldTranslation joint)))))) 1.470 + (.setLimit (float limit-xy) 1.471 + (float limit-yz) 1.472 + (float twist))))) 1.473 + true 1.474 + (println-repl 1.475 + "joint-type " joint-type " not recognized"))) 1.476 + 1.477 + (println-repl "could not find joint meta-data!"))))) 1.478 + 1.479 1.480 1.481 -(defn physical-hand [#^Node pieces joints] 1.482 - ;; Make each piece a physical entity in the simulation. 1.483 +(defn physical-worm [#^Node pieces joints] 1.484 (dorun 1.485 (map 1.486 (fn [geom] 1.487 @@ -156,44 +549,48 @@ 1.488 (RigidBodyControl. 1.489 (HullCollisionShape. 1.490 (.getMesh geom)) 1.491 - ;; TODO: fix this. 1.492 - (float 1.0))] 1.493 + (if-let [mass (meta-data geom "mass")] 1.494 + (do 1.495 + (println-repl 1.496 + "setting mass to " (float mass)) 1.497 + (float mass)) 1.498 + (float 1)))] 1.499 + 1.500 (.addControl geom physics-control))) 1.501 (filter #(isa? (class %) Geometry ) 1.502 (node-seq pieces)))) 1.503 + 1.504 (dorun 1.505 (map 1.506 - (fn [joint-position] 1.507 - (let [[geom-a geom-b] (find-joint pieces joint-position)] 1.508 - (connect-at-point geom-a geom-b joint-position))) 1.509 - joints)) 1.510 - pieces) 1.511 + (fn [joint] 1.512 + (let [[obj-a obj-b] 1.513 + (joint-targets pieces joint)] 1.514 + (connect obj-a obj-b joint))) 1.515 + joints)) 1.516 + pieces) 1.517 1.518 +(defn the-worm [] 1.519 + (physical-worm (worm) (worm-joints))) 1.520 1.521 -(defn the-hand! [] (physical-hand (hand) (hand-joints))) 1.522 - 1.523 - 1.524 -(defn test-hand [] 1.525 +(defn test-worm [] 1.526 (world 1.527 - (nodify [(the-hand!) 1.528 - (box 10 1 10 :position (Vector3f. 0 -10 0) 1.529 - :color ColorRGBA/Gray 1.530 - :mass 0)]) 1.531 + (nodify [(the-worm) 1.532 + (box 10 2 10 :position (Vector3f. 0 -5 0) 1.533 + :color ColorRGBA/Gray :mass 0)]) 1.534 standard-debug-controls 1.535 - enable-debug 1.536 - (fn [_ _] 1.537 - (Thread/sleep 100)))) 1.538 - 1.539 - 1.540 - 1.541 - 1.542 - 1.543 - 1.544 + (comp light-up-everything enable-debug 1.545 + (fn [world] 1.546 + (.setTimer world (NanoTimer.)) 1.547 + (speed-up world) 1.548 + ;;(set-gravity world (Vector3f. 0 0 0)) 1.549 + world 1.550 + )) 1.551 + no-op)) 1.552 1.553 #+end_src 1.554 1.555 #+results: body-1 1.556 -: #'cortex.silly/test-hand 1.557 +: #'cortex.silly/test-try-again 1.558 1.559 1.560