Mercurial > cortex

comparison org/body.org @ 137:39c89ae5c7d0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
saving progress
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 23:46:31 -0700
parents 47a4d74761f0
children 16bdf9e80daf
comparison
equal deleted inserted replaced
136:47a4d74761f0 137:39c89ae5c7d0
64 (do (println-repl "could not find JOINTS node") []))) 64 (do (println-repl "could not find JOINTS node") [])))
65 65
66 (defn joint-proprioception [#^Node parts #^Node joint] 66 (defn joint-proprioception [#^Node parts #^Node joint]
67 (let [[obj-a obj-b] (joint-targets parts joint) 67 (let [[obj-a obj-b] (joint-targets parts joint)
68 joint-rot (.getWorldRotation joint) 68 joint-rot (.getWorldRotation joint)
69 x (.mult joint-rot Vector3f/UNIT_X) 69 pre-inv-a (.inverse (.getWorldRotation obj-a))
70 y (.mult joint-rot Vector3f/UNIT_Y) 70 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
71 z (.mult joint-rot Vector3f/UNIT_Z)] 71 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
72 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))]
73 (println-repl "x:" x)
74 (println-repl "y:" y)
75 (println-repl "z:" z)
72 ;; this function will report proprioceptive information for the 76 ;; this function will report proprioceptive information for the
73 ;; joint. 77 ;; joint.
74 (fn [] 78 (fn []
75 ;; x is the "twist" axis, y and z are the "bend" axes 79 ;; x is the "twist" axis, y and z are the "bend" axes
76 (let [rot-a (.getWorldRotation obj-a) 80 (let [rot-a (.getWorldRotation obj-a)
339 (.mult (.getPhysicsRotation control) 343 (.mult (.getPhysicsRotation control)
340 (.mult (.normalize direction) (float force)))))) 344 (.mult (.normalize direction) (float force))))))
341 345
342 346
343 347
348 (defmacro with-movement
349 [object
350 [up down left right roll-up roll-down :as keyboard]
351 forces
352 [world-invocation
353 root-node
354 keymap
355 intilization
356 world-loop]]
357 (let [add-keypress
358 (fn [state keymap key]
359 `(merge ~keymap
360 {~key
361 (fn [_ pressed?#]
362 (reset! ~state pressed?#))}))
363 move-left? (gensym "move-left?")
364 move-right? (gensym "move-right?")
365 move-up? (gensym "move-up?")
366 move-down? (gensym "move-down?")
367 roll-left? (gensym "roll-left?")
368 roll-right? (gensym "roll-right?")
369 directions
370 [(Vector3f. 0 1 0)
371 (Vector3f. 0 -1 0)
372 (Vector3f. 0 0 1)
373 (Vector3f. 0 0 -1)
374 (Vector3f. -1 0 0)
375 (Vector3f. 1 0 0)]
376 symbols [move-left? move-right? move-up? move-down?
377 roll-left? roll-right?]
378
379 keymap* (vec (map #(add-keypress %1 keymap %2)
380 symbols
381 keyboard))
382
383 splice-loop (map (fn [sym force direction]
384 `(if (deref ~sym)
385 (tap ~object ~direction ~force)))
386 symbols directions forces)
387
388 world-loop* `(fn [world# tpf#]
389 (~world-loop world# tpf#)
390 ~@splice-loop)]
391
392 `(let [~move-up? (atom false)
393 ~move-down? (atom false)
394 ~move-left? (atom false)
395 ~move-right? (atom false)
396 ~roll-left? (atom false)
397 ~roll-right? (atom false)]
398 (~world-invocation
399 ~root-node
400 (reduce merge ~keymap*)
401 ~intilization
402 ~world-loop*)
403 )))
404
405
344 (defn test-proprioception 406 (defn test-proprioception
345 "Testing proprioception: 407 "Testing proprioception:
346 You should see two foating bars, and a printout of pitch, yaw, and 408 You should see two foating bars, and a printout of pitch, yaw, and
347 roll. Pressing key-r/key-t should move the blue bar up and down and 409 roll. Pressing key-r/key-t should move the blue bar up and down and
348 change only the value of pitch. key-f/key-g moves it side to side 410 change only the value of pitch. key-f/key-g moves it side to side
357 :position (Vector3f. 1.2 2 0) 419 :position (Vector3f. 1.2 2 0)
358 :physical? false) 420 :physical? false)
359 joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) 421 joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
360 creature (nodify [hand finger joint-node]) 422 creature (nodify [hand finger joint-node])
361 ;; ******************************************* 423 ;; *******************************************
362 rot (doto (Quaternion.) 424
363 (.fromAngleAxis (/ Math/PI 4)
364 (Vector3f. 0 0 -1)))
365 hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
366 :mass 0 :color ColorRGBA/Blue :rotation rot)
367 finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
368 :mass 1 :color ColorRGBA/Magenta :rotation rot)
369 joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
370 :position (Vector3f. 1.2 1.5 -3)
371 :physical? false :rotation rot)
372 joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
373 creature2 (nodify [hand2 finger2 joint-node2])
374 ;; *******************************************
375
376 floor (box 10 10 10 :position (Vector3f. 0 -15 0) 425 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
377 :mass 0 :color ColorRGBA/Gray) 426 :mass 0 :color ColorRGBA/Gray)
378 427
379 move-up? (atom false) 428 move-up? (atom false)
380 move-down? (atom false) 429 move-down? (atom false)
381 move-left? (atom false) 430 move-left? (atom false)
382 move-right? (atom false) 431 move-right? (atom false)
383 roll-left? (atom false) 432 roll-left? (atom false)
384 roll-right? (atom false) 433 roll-right? (atom false)
385 434
386 435
387 root (nodify [creature creature2 floor]) 436 root (nodify [creature floor])
388 prop (joint-proprioception creature joint-node) 437 prop (joint-proprioception creature joint-node)
389 prop-view (proprioception-debug-window)] 438 prop-view (proprioception-debug-window)]
390 439
391 440
392 441
393 (.setCollisionGroup 442 (.setCollisionGroup
394 (.getControl hand RigidBodyControl) 443 (.getControl hand RigidBodyControl)
395 PhysicsCollisionObject/COLLISION_GROUP_NONE) 444 PhysicsCollisionObject/COLLISION_GROUP_NONE)
396 (.setCollisionGroup 445
397 (.getControl hand2 RigidBodyControl) 446 (with-movement
398 PhysicsCollisionObject/COLLISION_GROUP_NONE) 447 finger
399 448 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
400 449 [10 10 10 10 1 1]
401 450 (world
402 451 root
403 (world 452 (merge standard-debug-controls
404 root 453 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
405 (merge standard-debug-controls 454 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
406 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 455 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
407 "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 456 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
408 "key-f" (fn [_ pressed?] (reset! move-left? pressed?)) 457 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
409 "key-g" (fn [_ pressed?] (reset! move-right? pressed?)) 458 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
410 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?)) 459 (fn [world]
411 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))}) 460 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
412 (fn [world] 461 (set-gravity world (Vector3f. 0 0 0))
413 (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) 462 (light-up-everything world))
414 (set-gravity world (Vector3f. 0 0 0)) 463 (fn [_ _]
415 (light-up-everything world)) 464 (let [force 10
416 (fn [_ _] 465 left (Vector3f. 0 1 0)
417 (let [force 10 466 right (Vector3f. 0 -1 0)
418 left (Vector3f. 0 1 0) 467 up (Vector3f. 0 0 1)
419 right (Vector3f. 0 -1 0) 468 down (Vector3f. 0 0 -1)
420 up (Vector3f. 0 0 1) 469 roll-left (Vector3f. -1 0 0)
421 down (Vector3f. 0 0 -1) 470 roll-right (Vector3f. 1 0 0)]
422 roll-left (Vector3f. -1 0 0) 471 (if @move-up? (tap finger up force))
423 roll-right (Vector3f. 1 0 0)] 472
424 (if @move-up? (do (tap finger up force) 473 (if @move-down? (tap finger down force))
425 (tap finger2 up force))) 474
426 (if @move-down? (do (tap finger down force) 475 (if @move-left? (tap finger left force))
427 (tap finger2 down force))) 476
428 (if @move-left? (do (tap finger left force) 477 (if @move-right? (tap finger right force))
429 (tap finger2 left force))) 478
430 (if @move-right? (do (tap finger right force) 479 (if @roll-left? (tap finger roll-left (/ force 10)))
431 (tap finger2 right force))) 480
432 (if @roll-left? (do (tap finger roll-left (/ force 10)) 481 (if @roll-right? (tap finger roll-right (/ force 10))))
433 (tap finger2 roll-left (/ force 10)))) 482
434 (if @roll-right? (do (tap finger roll-right (/ force 10)) 483 (prop-view (list (prop))))))))
435 (tap finger2 roll-right (/ force 10))))) 484
436 (prop-view (list (prop)))))))
437
438 #+end_src 485 #+end_src
439 486
440 #+results: test-body 487 #+results: test-body
441 : #'cortex.test.body/test-proprioception 488 : #'cortex.test.body/test-proprioception
442 489