Mercurial > cortex
view org/body.org @ 62:2b9d81017cb7
moved utility functions out of body and into util
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Mon, 28 Nov 2011 21:28:46 -0700 |
parents | 7b44348af538 |
children | 7f2653ad3199 |
line wrap: on
line source
1 #+title: The BODY!!!2 #+author: Robert McIntyre3 #+email: rlm@mit.edu4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.5 #+SETUPFILE: ../../aurellem/org/setup.org6 #+INCLUDE: ../../aurellem/org/level-0.org8 * Body10 #+srcname: body-main11 #+begin_src clojure12 (ns cortex.body13 (use (cortex world util import)))15 (use 'clojure.contrib.def)16 (cortex.import/mega-import-jme3)17 (rlm.rlm-commands/help)19 (defn load-blender-model20 [^String model]21 (.loadModel22 (doto (asset-manager)23 (.registerLoader BlenderModelLoader (into-array String ["blend"])))24 model))26 (defn skel [node]27 (doto28 (.getSkeleton29 (.getControl node SkeletonControl))30 ;; this is necessary to force the skeleton to have accurate world31 ;; transforms before it is rendered to the screen.32 (.resetAndUpdate)))35 (defn green-x-ray []36 (doto (Material. (asset-manager)37 "Common/MatDefs/Misc/Unshaded.j3md")38 (.setColor "Color" ColorRGBA/Green)39 (-> (.getAdditionalRenderState)40 (.setDepthTest false))))43 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;45 ;;;;;;;;;;;; eve-style bodies ;;;;;;;;47 (defn worm [segment-length num-segments interstitial-space radius]48 (letfn [(nth-segment49 [n]50 (box segment-length radius radius :mass 0.151 :position52 (Vector3f.53 (* 2 n (+ interstitial-space segment-length)) 0 0)54 :name (str "worm-segment" n)55 :color (ColorRGBA/randomColor)))]56 (map nth-segment (range num-segments))))59 (defn connect-at-midpoint60 [segmentA segmentB]61 (let [centerA (.getWorldTranslation segmentA)62 centerB (.getWorldTranslation segmentB)63 midpoint (.mult (.add centerA centerB) (float 0.5))64 pivotA (.subtract midpoint centerA)65 pivotB (.subtract midpoint centerB)66 joint (Point2PointJoint.67 (.getControl segmentA RigidBodyControl)68 (.getControl segmentB RigidBodyControl)69 pivotA70 pivotB)]71 segmentB))73 (defn point-worm []74 (let [segments (worm 0.2 5 0.1 0.1)]75 (dorun (map (partial apply connect-at-midpoint)76 (partition 2 1 segments)))77 (nodify "worm" segments)))79 (defn test-worm []80 (.start81 (world82 (doto (Node.)83 ;;(.attachChild (point-worm))84 (.attachChild (load-blender-model85 "Models/anim2/joint-worm.blend"))87 (.attachChild (box 10 1 1088 :position (Vector3f. 0 -2 0) :mass 089 :color (ColorRGBA/Gray))))90 {91 "key-space" (fire-cannon-ball)92 }93 (fn [world]94 (enable-debug world)95 (light-up-everything world)96 ;;(.setTimer world (NanoTimer.))97 )98 no-op)))100 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;103 ;;;;;;;;; Mortor Control ;;;;;;;;;;;;;106 ;; surprisingly ehough, terristerial creatures only move by using107 ;; torque applied to their joints. There's not a single straight line108 ;; of force in the human body at all! (a straight line of force would109 ;; correspond to some sort of jet or rocket propulseion)111 (defn torque-controls [control]112 (let [torques113 (concat114 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))115 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))116 [Vector3f/UNIT_X])]117 (map (fn [torque-axis]118 (fn [torque]119 (.applyTorque120 control121 (.mult (.mult (.getPhysicsRotation control)122 torque-axis)123 (float124 (* (.getMass control) torque))))))125 torques)))127 (defn motor-map128 "Take a creature and generate a function that will enable fine129 grained control over all the creature's limbs."130 [#^Node creature]131 (let [controls (keep #(.getControl % RigidBodyControl)132 (node-seq creature))133 limb-controls (reduce concat (map torque-controls controls))134 body-control (partial map #(%1 %2) limb-controls)]135 body-control))137 (defn test-motor-map138 "see how torque works."139 []140 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)141 :mass 1 :color ColorRGBA/Green)142 motor-map (motor-map finger)]143 (world144 (nodify [finger145 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0146 :color ColorRGBA/Gray)])147 standard-debug-controls148 (fn [world]149 (set-gravity world Vector3f/ZERO)150 (light-up-everything world)151 (.setTimer world (NanoTimer.)))152 (fn [_ _]153 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))155 (defn test-torque156 "see how torque works."157 []158 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)159 :mass 1 :color ColorRGBA/Green)160 move-left? (atom false)161 move-right? (atom false)162 control (.getControl finger RigidBodyControl)]163 (world164 (nodify [finger165 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0166 :color ColorRGBA/Gray)])167 (merge standard-debug-controls168 {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))169 "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})170 (fn [world]171 (set-gravity world Vector3f/ZERO)172 (light-up-everything world)173 (.setTimer world (NanoTimer.)))174 (fn [_ _]175 (if @move-left?176 (.applyTorque control177 (.mult (.getPhysicsRotation control)178 (Vector3f. -3 20 0))))179 (if @move-right?180 (.applyTorque control (Vector3f. 0 0 1)))))))182 (defn worm-pattern [time]183 [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0185 0 0 0 0 0 0 0 0 0 0 0187 (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))189 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0190 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0191 0 0 0 0 0 0 0 0 0 0 0 0 0 0193 ])195 ;;;;;;;;;;;;;;;;;; Proprioception ;;;;;;;;;;;;;;;;;;;;;;;;197 ;; this is not used as just getting the rotation would be simpler.198 (defn proprioception-senses199 "given a control , create a sequence of thunks that will report the200 rotation of the control's object along the same axes as the motor-control map."201 [control]202 (let [torques203 (concat204 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))205 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))206 [Vector3f/UNIT_X])]207 (map (fn [torque-axis]208 (fn []209 (.getPhysicsRotation control)))210 torques)))212 (defn orthogonal-vect213 "Return a vector orthogonal to the current one"214 [vector]215 (let [x (.getX vector)216 y (.getY vector)217 z (.getZ vector)]218 (cond219 (not= x (float 0)) (Vector3f. (- z) 0 x)220 (not= y (float 0)) (Vector3f. 0 (- z) y)221 (not= z (float 0)) (Vector3f. 0 (- z) y)222 true Vector3f/ZERO)))224 ;; from225 ;; http://stackoverflow.com/questions/3684269/ \\226 ;; component-of-a-quaternion-rotation-around-an-axis227 (defn rot-about-axis [#^Quaternion q #^Vector3f axis]228 (let [basis-1 (orthogonal-vect axis)229 basis-2 (.cross axis basis-1)230 rotated (.mult q basis-1)231 alpha (.dot basis-1 (.project rotated basis-1))232 beta (.dot basis-2 (.project rotated basis-2))]233 (Math/atan2 beta alpha)))236 (defn check-rot [a]237 (rot-about-axis238 (doto (Quaternion.)239 (.fromAngleAxis240 (float a)241 (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))243 (defn relative-positions [joint]244 (let [object-a (.getUserObject (.getBodyA joint))245 object-b (.getUserObject (.getBodyB joint))246 arm-a247 (.normalize248 (.subtract249 (.localToWorld object-a (.getPivotA joint) nil)250 (.getWorldTranslation object-a)))251 rotate-a252 (doto (Matrix3f.)253 (.fromStartEndVectors arm-a Vector3f/UNIT_X))254 arm-b255 (.mult256 rotate-a257 (.normalize258 (.subtract259 (.localToWorld object-b (.getPivotB joint) nil)260 (.getWorldTranslation object-b))))261 pitch262 (.angleBetween263 (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))264 (Vector2f. 1 0))265 yaw266 (.angleBetween267 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))268 (Vector2f. 1 0))270 roll271 (rot-about-axis272 (.mult273 (.getLocalRotation object-b)274 (doto (Quaternion.)275 (.fromRotationMatrix rotate-a)))276 arm-b)277 ]281 ;;(println-repl282 ;; "arm-b is " arm-b)283 ;;(println-repl284 ;; "pivot-b is " (.getPivotB joint))285 ;;(println-repl286 ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"287 ;; pitch yaw roll))288 [pitch yaw roll]))296 (defn test-worm-control297 []298 (let [worm (point-worm)299 time (atom 0)300 worm-motor-map (motor-map worm)301 ;;body-map (proprioception worm)302 debug-segments303 (map304 #(doto305 (make-shape306 (assoc base-shape307 :name "debug-line"308 :physical? false309 :shape310 (com.jme3.scene.shape.Line.311 (.add (.getWorldTranslation %)312 (Vector3f. -0.2 0 0 ))313 (.add (.getWorldTranslation %)314 (Vector3f. 0.2 0 0)))))315 (.setMaterial (green-x-ray)))316 (drop 1 (node-seq worm)))]317 (world318 (nodify [worm319 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0320 :color ColorRGBA/Gray)])321 standard-debug-controls322 (fn [world]323 (.attachChild (.getRootNode world) (nodify debug-segments))324 (enable-debug world)325 (light-up-everything world)326 (com.aurellem.capture.Capture/captureVideo327 world328 (file-str "/home/r/proj/cortex/tmp/moving-worm")))330 (fn [_ _]331 (dorun332 (map333 (fn [worm-segment334 debug-segment]335 (.rotate336 debug-segment337 (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))338 (drop 1 (node-seq worm))339 debug-segments))340 (swap! time inc)341 ;;(println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))342 (Thread/sleep 200)343 (dorun (worm-motor-map344 (worm-pattern @time)))))))350 (defn test-prop351 "see how torque works."352 []353 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)354 :mass 0 :color ColorRGBA/Green)355 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)356 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))357 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0)358 :mass 0 :color ColorRGBA/Gray)360 move-up? (atom false)361 move-down? (atom false)362 move-left? (atom false)363 move-right? (atom false)364 roll-left? (atom false)365 roll-right? (atom false)366 control (.getControl finger RigidBodyControl)367 joint368 (doto369 (Point2PointJoint.370 (.getControl hand RigidBodyControl)371 control372 (Vector3f. 1.2 0 0)373 (Vector3f. -1.2 0 0 ))374 (.setCollisionBetweenLinkedBodys false))375 time (atom 0)376 ]377 (world378 (nodify [hand finger floor])379 (merge standard-debug-controls380 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))381 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))382 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))383 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))384 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))385 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})386 (fn [world]387 (set-gravity world (Vector3f. 0 0 0))388 (.setMoveSpeed (.getFlyByCamera world) 50)389 (.setRotationSpeed (.getFlyByCamera world) 50)390 (light-up-everything world)391 (.setTimer world (NanoTimer.))392 )393 (fn [_ _]394 (if @move-up?395 (.applyTorque control396 (.mult (.getPhysicsRotation control)397 (Vector3f. 0 0 10))))398 (if @move-down?399 (.applyTorque control400 (.mult (.getPhysicsRotation control)401 (Vector3f. 0 0 -10))))402 (if @move-left?403 (.applyTorque control404 (.mult (.getPhysicsRotation control)405 (Vector3f. 0 10 0))))406 (if @move-right?407 (.applyTorque control408 (.mult (.getPhysicsRotation control)409 (Vector3f. 0 -10 0))))410 (if @roll-left?411 (.applyTorque control412 (.mult (.getPhysicsRotation control)413 (Vector3f. -1 0 0))))414 (if @roll-right?415 (.applyTorque control416 (.mult (.getPhysicsRotation control)417 (Vector3f. 1 0 0))))419 (if (= 0 (rem (swap! time inc) 2000))420 (do422 (apply423 (comp424 println-repl425 #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))426 (relative-positions joint))))))))428 #+end_src431 * COMMENT failed-clojure-code432 #+begin_src clojure433 ;;(.loadModel434 ;; (doto (asset-manager)435 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))436 ;; "Models/person/person.blend")438 (defn view-model [^String model]439 (view440 (.loadModel441 (doto (asset-manager)442 (.registerLoader BlenderModelLoader (into-array String ["blend"])))443 model)))445 (defn load-blender-scene [^String model]446 (.loadModel447 (doto (asset-manager)448 (.registerLoader BlenderLoader (into-array String ["blend"])))449 model))451 (defn worm452 []453 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))455 (defn oto456 []457 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))459 (defn sinbad460 []461 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))463 (defn worm-blender464 []465 (first (seq (.getChildren (load-blender-model466 "Models/anim2/simple-worm.blend")))))468 (defn body469 "given a node with a SkeletonControl, will produce a body sutiable470 for AI control with movement and proprioception."471 [node]472 (let [skeleton-control (.getControl node SkeletonControl)473 krc (KinematicRagdollControl.)]474 (comment475 (dorun476 (map #(.addBoneName krc %)477 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]478 ;;"mid2" "mid3" "tail" "head"]479 )))480 (.addControl node krc)481 (.setRagdollMode krc)482 )483 node484 )485 (defn show-skeleton [node]486 (let [sd488 (doto489 (SkeletonDebugger. "aurellem-skel-debug"490 (skel node))491 (.setMaterial (green-x-ray)))]492 (.attachChild node sd)493 node))497 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;499 ;; this could be a good way to give objects special properties like500 ;; being eyes and the like502 (.getUserData503 (.getChild504 (load-blender-model "Models/property/test.blend") 0)505 "properties")507 ;; the properties are saved along with the blender file.508 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;513 (defn init-debug-skel-node514 [f debug-node skeleton]515 (let [bones516 (map #(.getBone skeleton %)517 (range (.getBoneCount skeleton)))]518 (dorun (map #(.setUserControl % true) bones))519 (dorun (map (fn [b]520 (println (.getName b)521 " -- " (f b)))522 bones))523 (dorun524 (map #(.attachChild525 debug-node526 (doto527 (sphere 0.1528 :position (f %)529 :physical? false)530 (.setMaterial (green-x-ray))))531 bones)))532 debug-node)534 (import jme3test.bullet.PhysicsTestHelper)537 (defn test-zzz [the-worm world value]538 (if (not value)539 (let [skeleton (skel the-worm)]540 (println-repl "enabling bones")541 (dorun542 (map543 #(.setUserControl (.getBone skeleton %) true)544 (range (.getBoneCount skeleton))))547 (let [b (.getBone skeleton 2)]548 (println-repl "moving " (.getName b))549 (println-repl (.getLocalPosition b))550 (.setUserTransforms b551 Vector3f/UNIT_X552 Quaternion/IDENTITY553 ;;(doto (Quaternion.)554 ;; (.fromAngles (/ Math/PI 2)555 ;; 0556 ;; 0558 (Vector3f. 1 1 1))559 )561 (println-repl "hi! <3"))))564 (defn test-ragdoll []566 (let [the-worm568 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")569 (doto (show-skeleton (worm-blender))570 (.setLocalTranslation (Vector3f. 0 10 0))571 ;;(worm)572 ;;(oto)573 ;;(sinbad)574 )575 ]578 (.start579 (world580 (doto (Node.)581 (.attachChild the-worm))582 {"key-return" (fire-cannon-ball)583 "key-space" (partial test-zzz the-worm)584 }585 (fn [world]586 (light-up-everything world)587 (PhysicsTestHelper/createPhysicsTestWorld588 (.getRootNode world)589 (asset-manager)590 (.getPhysicsSpace591 (.getState (.getStateManager world) BulletAppState)))592 (set-gravity world Vector3f/ZERO)593 ;;(.setTimer world (NanoTimer.))594 ;;(org.lwjgl.input.Mouse/setGrabbed false)595 )596 no-op597 )600 )))603 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;604 ;;; here is the ragdoll stuff606 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))607 (def mesh worm-mesh)609 (.getFloatBuffer mesh VertexBuffer$Type/Position)610 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)611 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))614 (defn position [index]615 (.get616 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)617 index))619 (defn bones [index]620 (.get621 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))622 index))624 (defn bone-weights [index]625 (.get626 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)627 index))631 (defn vertex-bones [vertex]632 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))634 (defn vertex-weights [vertex]635 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))637 (defn vertex-position [index]638 (let [offset (* index 3)]639 (Vector3f. (position offset)640 (position (inc offset))641 (position (inc(inc offset))))))643 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))645 (defn bone-control-color [index]646 (get {[1 0 0 0] ColorRGBA/Red647 [1 2 0 0] ColorRGBA/Magenta648 [2 0 0 0] ColorRGBA/Blue}649 (vertex-bones index)650 ColorRGBA/White))652 (defn influence-color [index bone-num]653 (get654 {(float 0) ColorRGBA/Blue655 (float 0.5) ColorRGBA/Green656 (float 1) ColorRGBA/Red}657 ;; find the weight of the desired bone658 ((zipmap (vertex-bones index)(vertex-weights index))659 bone-num)660 ColorRGBA/Blue))662 (def worm-vertices (set (map vertex-info (range 60))))665 (defn test-info []666 (let [points (Node.)]667 (dorun668 (map #(.attachChild points %)669 (map #(sphere 0.01670 :position (vertex-position %)671 :color (influence-color % 1)672 :physical? false)673 (range 60))))674 (view points)))677 (defrecord JointControl [joint physics-space]678 PhysicsControl679 (setPhysicsSpace [this space]680 (dosync681 (ref-set (:physics-space this) space))682 (.addJoint space (:joint this)))683 (update [this tpf])684 (setSpatial [this spatial])685 (render [this rm vp])686 (getPhysicsSpace [this] (deref (:physics-space this)))687 (isEnabled [this] true)688 (setEnabled [this state]))690 (defn add-joint691 "Add a joint to a particular object. When the object is added to the692 PhysicsSpace of a simulation, the joint will also be added"693 [object joint]694 (let [control (JointControl. joint (ref nil))]695 (.addControl object control))696 object)699 (defn hinge-world700 []701 (let [sphere1 (sphere)702 sphere2 (sphere 1 :position (Vector3f. 3 3 3))703 joint (Point2PointJoint.704 (.getControl sphere1 RigidBodyControl)705 (.getControl sphere2 RigidBodyControl)706 Vector3f/ZERO (Vector3f. 3 3 3))]707 (add-joint sphere1 joint)708 (doto (Node. "hinge-world")709 (.attachChild sphere1)710 (.attachChild sphere2))))713 (defn test-joint []714 (view (hinge-world)))716 ;; (defn copier-gen []717 ;; (let [count (atom 0)]718 ;; (fn [in]719 ;; (swap! count inc)720 ;; (clojure.contrib.duck-streams/copy721 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"722 ;; ;;/home/r/tmp/mao-test/clojure-images723 ;; (format "%08d.png" @count)))))))724 ;; (defn decrease-framerate []725 ;; (map726 ;; (copier-gen)727 ;; (sort728 ;; (map first729 ;; (partition730 ;; 4731 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))732 ;; (file-seq733 ;; (file-str734 ;; "/home/r/media/anime/mao-temp/images"))))))))738 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;740 (defn proprioception741 "Create a proprioception map that reports the rotations of the742 various limbs of the creature's body"743 [creature]744 [#^Node creature]745 (let [746 nodes (node-seq creature)747 joints748 (map749 :joint750 (filter751 #(isa? (class %) JointControl)752 (reduce753 concat754 (map (fn [node]755 (map (fn [num] (.getControl node num))756 (range (.getNumControls node))))757 nodes))))]758 (fn []759 (reduce concat (map relative-positions (list (first joints)))))))762 #+end_src770 * COMMENT generate Source.771 #+begin_src clojure :tangle ../src/cortex/body.clj772 <<body-main>>773 #+end_src