view org/body.org @ 159:75b6c2ebbf8e

refactored audio code
author Robert McIntyre <rlm@mit.edu>
date Fri, 03 Feb 2012 06:41:16 -0700
parents 84c67be00abe
children 33278bf028e7
line wrap: on
line source
1 #+title: The BODY!!!
2 #+author: Robert McIntyre
3 #+email: rlm@mit.edu
4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
5 #+SETUPFILE: ../../aurellem/org/setup.org
6 #+INCLUDE: ../../aurellem/org/level-0.org
8 * Proprioception
9 #+name: proprioception
10 #+begin_src clojure
11 (ns cortex.body
12 (:use (cortex world util))
13 (:import
14 com.jme3.math.Vector3f
15 com.jme3.math.Quaternion
16 com.jme3.math.Vector2f
17 com.jme3.math.Matrix3f
18 com.jme3.bullet.control.RigidBodyControl
19 com.jme3.collision.CollisionResults
20 com.jme3.bounding.BoundingBox
21 com.jme3.scene.Node))
23 (defn jme-to-blender
24 "Convert from JME coordinates to Blender coordinates"
25 [#^Vector3f in]
26 (Vector3f. (.getX in)
27 (- (.getZ in))
28 (.getY in)))
30 (defn joint-targets
31 "Return the two closest two objects to the joint object, ordered
32 from bottom to top according to the joint's rotation."
33 [#^Node parts #^Node joint]
34 (loop [radius (float 0.01)]
35 (let [results (CollisionResults.)]
36 (.collideWith
37 parts
38 (BoundingBox. (.getWorldTranslation joint)
39 radius radius radius)
40 results)
41 (let [targets
42 (distinct
43 (map #(.getGeometry %) results))]
44 (if (>= (count targets) 2)
45 (sort-by
46 #(let [v
47 (jme-to-blender
48 (.mult
49 (.inverse (.getWorldRotation joint))
50 (.subtract (.getWorldTranslation %)
51 (.getWorldTranslation joint))))]
52 (println-repl (.getName %) ":" v)
53 (.dot (Vector3f. 1 1 1)
54 v))
55 (take 2 targets))
56 (recur (float (* radius 2))))))))
58 (defn creature-joints
59 "Return the children of the creature's \"joints\" node."
60 [#^Node creature]
61 (if-let [joint-node (.getChild creature "joints")]
62 (seq (.getChildren joint-node))
63 (do (println-repl "could not find JOINTS node") [])))
65 (defn tap [obj direction force]
66 (let [control (.getControl obj RigidBodyControl)]
67 (.applyTorque
68 control
69 (.mult (.getPhysicsRotation control)
70 (.mult (.normalize direction) (float force))))))
73 (defn with-movement
74 [object
75 [up down left right roll-up roll-down :as keyboard]
76 forces
77 [root-node
78 keymap
79 intilization
80 world-loop]]
81 (let [add-keypress
82 (fn [state keymap key]
83 (merge keymap
84 {key
85 (fn [_ pressed?]
86 (reset! state pressed?))}))
87 move-up? (atom false)
88 move-down? (atom false)
89 move-left? (atom false)
90 move-right? (atom false)
91 roll-left? (atom false)
92 roll-right? (atom false)
94 directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0)
95 (Vector3f. 0 0 1)(Vector3f. 0 0 -1)
96 (Vector3f. -1 0 0)(Vector3f. 1 0 0)]
97 atoms [move-left? move-right? move-up? move-down?
98 roll-left? roll-right?]
100 keymap* (reduce merge
101 (map #(add-keypress %1 keymap %2)
102 atoms
103 keyboard))
105 splice-loop (fn []
106 (dorun
107 (map
108 (fn [sym direction force]
109 (if @sym
110 (tap object direction force)))
111 atoms directions forces)))
113 world-loop* (fn [world tpf]
114 (world-loop world tpf)
115 (splice-loop))]
116 [root-node
117 keymap*
118 intilization
119 world-loop*]))
121 (import java.awt.image.BufferedImage)
123 (defn draw-sprite [image sprite x y color ]
124 (dorun
125 (for [[u v] sprite]
126 (.setRGB image (+ u x) (+ v y) color))))
128 (defn view-angle
129 "create a debug view of an angle"
130 [color]
131 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
132 previous (atom [25 25])
133 sprite [[0 0] [0 1]
134 [0 -1] [-1 0] [1 0]]]
135 (fn [angle]
136 (let [angle (float angle)]
137 (let [position
138 [(+ 25 (int (* 20 (Math/cos angle))))
139 (+ 25 (int (* -20 (Math/sin angle))))]]
140 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
141 (draw-sprite image sprite (position 0) (position 1) color)
142 (reset! previous position))
143 image))))
145 (defn proprioception-debug-window
146 []
147 (let [heading (view-angle 0xFF0000)
148 pitch (view-angle 0x00FF00)
149 roll (view-angle 0xFFFFFF)
150 v-heading (view-image)
151 v-pitch (view-image)
152 v-roll (view-image)
153 ]
154 (fn [prop-data]
155 (dorun
156 (map
157 (fn [[h p r]]
158 (v-heading (heading h))
159 (v-pitch (pitch p))
160 (v-roll (roll r)))
161 prop-data)))))
164 #+end_src
166 #+results: proprioception
167 : #'cortex.body/proprioception-debug-window
169 * Motor Control
170 #+name: motor-control
171 #+begin_src clojure
172 (in-ns 'cortex.body)
174 ;; surprisingly enough, terristerial creatures only move by using
175 ;; torque applied about their joints. There's not a single straight
176 ;; line of force in the human body at all! (A straight line of force
177 ;; would correspond to some sort of jet or rocket propulseion.)
179 (defn vector-motor-control
180 "Create a function that accepts a sequence of Vector3f objects that
181 describe the torque to be applied to each part of the body."
182 [body]
183 (let [nodes (node-seq body)
184 controls (keep #(.getControl % RigidBodyControl) nodes)]
185 (fn [torques]
186 (map #(.applyTorque %1 %2)
187 controls torques))))
188 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
189 #+end_src
191 ## note -- might want to add a lower dimensional, discrete version of
192 ## this if it proves useful from a x-modal clustering perspective.
194 * Examples
196 #+name: test-body
197 #+begin_src clojure
198 (ns cortex.test.body
199 (:use (cortex world util body))
200 (:require cortex.silly)
201 (:import
202 com.jme3.math.Vector3f
203 com.jme3.math.ColorRGBA
204 com.jme3.bullet.joints.Point2PointJoint
205 com.jme3.bullet.control.RigidBodyControl
206 com.jme3.system.NanoTimer
207 com.jme3.math.Quaternion))
209 (defn worm-segments
210 "Create multiple evenly spaced box segments. They're fabulous!"
211 [segment-length num-segments interstitial-space radius]
212 (letfn [(nth-segment
213 [n]
214 (box segment-length radius radius :mass 0.1
215 :position
216 (Vector3f.
217 (* 2 n (+ interstitial-space segment-length)) 0 0)
218 :name (str "worm-segment" n)
219 :color (ColorRGBA/randomColor)))]
220 (map nth-segment (range num-segments))))
222 (defn connect-at-midpoint
223 "Connect two physics objects with a Point2Point joint constraint at
224 the point equidistant from both objects' centers."
225 [segmentA segmentB]
226 (let [centerA (.getWorldTranslation segmentA)
227 centerB (.getWorldTranslation segmentB)
228 midpoint (.mult (.add centerA centerB) (float 0.5))
229 pivotA (.subtract midpoint centerA)
230 pivotB (.subtract midpoint centerB)
232 ;; A side-effect of creating a joint registers
233 ;; it with both physics objects which in turn
234 ;; will register the joint with the physics system
235 ;; when the simulation is started.
236 joint (Point2PointJoint.
237 (.getControl segmentA RigidBodyControl)
238 (.getControl segmentB RigidBodyControl)
239 pivotA
240 pivotB)]
241 segmentB))
243 (defn eve-worm
244 "Create a worm-like body bound by invisible joint constraints."
245 []
246 (let [segments (worm-segments 0.2 5 0.1 0.1)]
247 (dorun (map (partial apply connect-at-midpoint)
248 (partition 2 1 segments)))
249 (nodify "worm" segments)))
251 (defn worm-pattern
252 "This is a simple, mindless motor control pattern that drives the
253 second segment of the worm's body at an offset angle with
254 sinusoidally varying strength."
255 [time]
256 (let [angle (* Math/PI (/ 9 20))
257 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
258 [Vector3f/ZERO
259 (.mult
260 direction
261 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
262 Vector3f/ZERO
263 Vector3f/ZERO
264 Vector3f/ZERO]))
266 (defn test-motor-control
267 "Testing motor-control:
268 You should see a multi-segmented worm-like object fall onto the
269 table and begin writhing and moving."
270 []
271 (let [worm (eve-worm)
272 time (atom 0)
273 worm-motor-map (vector-motor-control worm)]
274 (world
275 (nodify [worm
276 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
277 :color ColorRGBA/Gray)])
278 standard-debug-controls
279 (fn [world]
280 (enable-debug world)
281 (light-up-everything world)
282 (comment
283 (com.aurellem.capture.Capture/captureVideo
284 world
285 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
286 )
288 (fn [_ _]
289 (swap! time inc)
290 (Thread/sleep 20)
291 (dorun (worm-motor-map
292 (worm-pattern @time)))))))
296 (defn join-at-point [obj-a obj-b world-pivot]
297 (cortex.silly/joint-dispatch
298 {:type :point}
299 (.getControl obj-a RigidBodyControl)
300 (.getControl obj-b RigidBodyControl)
301 (cortex.silly/world-to-local obj-a world-pivot)
302 (cortex.silly/world-to-local obj-b world-pivot)
303 nil
304 ))
306 (import com.jme3.bullet.collision.PhysicsCollisionObject)
308 (defn blab-* []
309 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
310 :mass 0 :color ColorRGBA/Green)
311 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
312 :mass 1 :color ColorRGBA/Red)
313 connection-point (Vector3f. 1.2 0 0)
314 root (nodify [hand finger])]
316 (join-at-point hand finger (Vector3f. 1.2 0 0))
318 (.setCollisionGroup
319 (.getControl hand RigidBodyControl)
320 PhysicsCollisionObject/COLLISION_GROUP_NONE)
321 (world
322 root
323 standard-debug-controls
324 (fn [world]
325 (enable-debug world)
326 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
327 (set-gravity world Vector3f/ZERO)
328 )
329 no-op)))
330 (comment
332 (defn proprioception-debug-window
333 []
334 (let [time (atom 0)]
335 (fn [prop-data]
336 (if (= 0 (rem (swap! time inc) 40))
337 (println-repl prop-data)))))
338 )
340 (comment
341 (dorun
342 (map
343 (comp
344 println-repl
345 (fn [[p y r]]
346 (format
347 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
348 p y r)))
349 prop-data)))
354 (defn test-proprioception
355 "Testing proprioception:
356 You should see two foating bars, and a printout of pitch, yaw, and
357 roll. Pressing key-r/key-t should move the blue bar up and down and
358 change only the value of pitch. key-f/key-g moves it side to side
359 and changes yaw. key-v/key-b will spin the blue segment clockwise
360 and counterclockwise, and only affect roll."
361 []
362 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
363 :mass 0 :color ColorRGBA/Green :name "hand")
364 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
365 :mass 1 :color ColorRGBA/Red :name "finger")
366 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
367 :position (Vector3f. 0 1.2 0)
368 :rotation (doto (Quaternion.)
369 (.fromAngleAxis
370 (/ Math/PI 2)
371 (Vector3f. 0 0 1)))
372 :physical? false)
373 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
374 creature (nodify [hand finger joint-node])
375 finger-control (.getControl finger RigidBodyControl)
376 hand-control (.getControl hand RigidBodyControl)]
379 (let
380 ;; *******************************************
382 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
383 :mass 0 :color ColorRGBA/Gray)
385 root (nodify [creature floor])
386 prop (joint-proprioception creature joint-node)
387 prop-view (proprioception-debug-window)
389 controls
390 (merge standard-debug-controls
391 {"key-o"
392 (fn [_ _] (.setEnabled finger-control true))
393 "key-p"
394 (fn [_ _] (.setEnabled finger-control false))
395 "key-k"
396 (fn [_ _] (.setEnabled hand-control true))
397 "key-l"
398 (fn [_ _] (.setEnabled hand-control false))
399 "key-i"
400 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
401 "key-period"
402 (fn [world _]
403 (.setEnabled finger-control false)
404 (.setEnabled hand-control false)
405 (.rotate creature (doto (Quaternion.)
406 (.fromAngleAxis
407 (float (/ Math/PI 15))
408 (Vector3f. 0 0 -1))))
410 (.setEnabled finger-control true)
411 (.setEnabled hand-control true)
412 (set-gravity world (Vector3f. 0 0 0))
413 )
416 }
417 )
419 ]
420 (comment
421 (.setCollisionGroup
422 (.getControl hand RigidBodyControl)
423 PhysicsCollisionObject/COLLISION_GROUP_NONE)
424 )
425 (apply
426 world
427 (with-movement
428 hand
429 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
430 [10 10 10 10 1 1]
431 (with-movement
432 finger
433 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
434 [1 1 10 10 10 10]
435 [root
436 controls
437 (fn [world]
438 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
439 (set-gravity world (Vector3f. 0 0 0))
440 (light-up-everything world))
441 (fn [_ _] (prop-view (list (prop))))]))))))
443 #+end_src
445 #+results: test-body
446 : #'cortex.test.body/test-proprioception
449 * COMMENT code-limbo
450 #+begin_src clojure
451 ;;(.loadModel
452 ;; (doto (asset-manager)
453 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
454 ;; "Models/person/person.blend")
457 (defn load-blender-model
458 "Load a .blend file using an asset folder relative path."
459 [^String model]
460 (.loadModel
461 (doto (asset-manager)
462 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
463 model))
466 (defn view-model [^String model]
467 (view
468 (.loadModel
469 (doto (asset-manager)
470 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
471 model)))
473 (defn load-blender-scene [^String model]
474 (.loadModel
475 (doto (asset-manager)
476 (.registerLoader BlenderLoader (into-array String ["blend"])))
477 model))
479 (defn worm
480 []
481 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
483 (defn oto
484 []
485 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
487 (defn sinbad
488 []
489 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
491 (defn worm-blender
492 []
493 (first (seq (.getChildren (load-blender-model
494 "Models/anim2/simple-worm.blend")))))
496 (defn body
497 "given a node with a SkeletonControl, will produce a body sutiable
498 for AI control with movement and proprioception."
499 [node]
500 (let [skeleton-control (.getControl node SkeletonControl)
501 krc (KinematicRagdollControl.)]
502 (comment
503 (dorun
504 (map #(.addBoneName krc %)
505 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
506 ;;"mid2" "mid3" "tail" "head"]
507 )))
508 (.addControl node krc)
509 (.setRagdollMode krc)
510 )
511 node
512 )
513 (defn show-skeleton [node]
514 (let [sd
516 (doto
517 (SkeletonDebugger. "aurellem-skel-debug"
518 (skel node))
519 (.setMaterial (green-x-ray)))]
520 (.attachChild node sd)
521 node))
525 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
527 ;; this could be a good way to give objects special properties like
528 ;; being eyes and the like
530 (.getUserData
531 (.getChild
532 (load-blender-model "Models/property/test.blend") 0)
533 "properties")
535 ;; the properties are saved along with the blender file.
536 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
541 (defn init-debug-skel-node
542 [f debug-node skeleton]
543 (let [bones
544 (map #(.getBone skeleton %)
545 (range (.getBoneCount skeleton)))]
546 (dorun (map #(.setUserControl % true) bones))
547 (dorun (map (fn [b]
548 (println (.getName b)
549 " -- " (f b)))
550 bones))
551 (dorun
552 (map #(.attachChild
553 debug-node
554 (doto
555 (sphere 0.1
556 :position (f %)
557 :physical? false)
558 (.setMaterial (green-x-ray))))
559 bones)))
560 debug-node)
562 (import jme3test.bullet.PhysicsTestHelper)
565 (defn test-zzz [the-worm world value]
566 (if (not value)
567 (let [skeleton (skel the-worm)]
568 (println-repl "enabling bones")
569 (dorun
570 (map
571 #(.setUserControl (.getBone skeleton %) true)
572 (range (.getBoneCount skeleton))))
575 (let [b (.getBone skeleton 2)]
576 (println-repl "moving " (.getName b))
577 (println-repl (.getLocalPosition b))
578 (.setUserTransforms b
579 Vector3f/UNIT_X
580 Quaternion/IDENTITY
581 ;;(doto (Quaternion.)
582 ;; (.fromAngles (/ Math/PI 2)
583 ;; 0
584 ;; 0
586 (Vector3f. 1 1 1))
587 )
589 (println-repl "hi! <3"))))
592 (defn test-ragdoll []
594 (let [the-worm
596 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
597 (doto (show-skeleton (worm-blender))
598 (.setLocalTranslation (Vector3f. 0 10 0))
599 ;;(worm)
600 ;;(oto)
601 ;;(sinbad)
602 )
603 ]
606 (.start
607 (world
608 (doto (Node.)
609 (.attachChild the-worm))
610 {"key-return" (fire-cannon-ball)
611 "key-space" (partial test-zzz the-worm)
612 }
613 (fn [world]
614 (light-up-everything world)
615 (PhysicsTestHelper/createPhysicsTestWorld
616 (.getRootNode world)
617 (asset-manager)
618 (.getPhysicsSpace
619 (.getState (.getStateManager world) BulletAppState)))
620 (set-gravity world Vector3f/ZERO)
621 ;;(.setTimer world (NanoTimer.))
622 ;;(org.lwjgl.input.Mouse/setGrabbed false)
623 )
624 no-op
625 )
628 )))
631 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
632 ;;; here is the ragdoll stuff
634 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
635 (def mesh worm-mesh)
637 (.getFloatBuffer mesh VertexBuffer$Type/Position)
638 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
639 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
642 (defn position [index]
643 (.get
644 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
645 index))
647 (defn bones [index]
648 (.get
649 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
650 index))
652 (defn bone-weights [index]
653 (.get
654 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
655 index))
659 (defn vertex-bones [vertex]
660 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
662 (defn vertex-weights [vertex]
663 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
665 (defn vertex-position [index]
666 (let [offset (* index 3)]
667 (Vector3f. (position offset)
668 (position (inc offset))
669 (position (inc(inc offset))))))
671 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
673 (defn bone-control-color [index]
674 (get {[1 0 0 0] ColorRGBA/Red
675 [1 2 0 0] ColorRGBA/Magenta
676 [2 0 0 0] ColorRGBA/Blue}
677 (vertex-bones index)
678 ColorRGBA/White))
680 (defn influence-color [index bone-num]
681 (get
682 {(float 0) ColorRGBA/Blue
683 (float 0.5) ColorRGBA/Green
684 (float 1) ColorRGBA/Red}
685 ;; find the weight of the desired bone
686 ((zipmap (vertex-bones index)(vertex-weights index))
687 bone-num)
688 ColorRGBA/Blue))
690 (def worm-vertices (set (map vertex-info (range 60))))
693 (defn test-info []
694 (let [points (Node.)]
695 (dorun
696 (map #(.attachChild points %)
697 (map #(sphere 0.01
698 :position (vertex-position %)
699 :color (influence-color % 1)
700 :physical? false)
701 (range 60))))
702 (view points)))
705 (defrecord JointControl [joint physics-space]
706 PhysicsControl
707 (setPhysicsSpace [this space]
708 (dosync
709 (ref-set (:physics-space this) space))
710 (.addJoint space (:joint this)))
711 (update [this tpf])
712 (setSpatial [this spatial])
713 (render [this rm vp])
714 (getPhysicsSpace [this] (deref (:physics-space this)))
715 (isEnabled [this] true)
716 (setEnabled [this state]))
718 (defn add-joint
719 "Add a joint to a particular object. When the object is added to the
720 PhysicsSpace of a simulation, the joint will also be added"
721 [object joint]
722 (let [control (JointControl. joint (ref nil))]
723 (.addControl object control))
724 object)
727 (defn hinge-world
728 []
729 (let [sphere1 (sphere)
730 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
731 joint (Point2PointJoint.
732 (.getControl sphere1 RigidBodyControl)
733 (.getControl sphere2 RigidBodyControl)
734 Vector3f/ZERO (Vector3f. 3 3 3))]
735 (add-joint sphere1 joint)
736 (doto (Node. "hinge-world")
737 (.attachChild sphere1)
738 (.attachChild sphere2))))
741 (defn test-joint []
742 (view (hinge-world)))
744 ;; (defn copier-gen []
745 ;; (let [count (atom 0)]
746 ;; (fn [in]
747 ;; (swap! count inc)
748 ;; (clojure.contrib.duck-streams/copy
749 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
750 ;; ;;/home/r/tmp/mao-test/clojure-images
751 ;; (format "%08d.png" @count)))))))
752 ;; (defn decrease-framerate []
753 ;; (map
754 ;; (copier-gen)
755 ;; (sort
756 ;; (map first
757 ;; (partition
758 ;; 4
759 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
760 ;; (file-seq
761 ;; (file-str
762 ;; "/home/r/media/anime/mao-temp/images"))))))))
766 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
768 (defn proprioception
769 "Create a proprioception map that reports the rotations of the
770 various limbs of the creature's body"
771 [creature]
772 [#^Node creature]
773 (let [
774 nodes (node-seq creature)
775 joints
776 (map
777 :joint
778 (filter
779 #(isa? (class %) JointControl)
780 (reduce
781 concat
782 (map (fn [node]
783 (map (fn [num] (.getControl node num))
784 (range (.getNumControls node))))
785 nodes))))]
786 (fn []
787 (reduce concat (map relative-positions (list (first joints)))))))
790 (defn skel [node]
791 (doto
792 (.getSkeleton
793 (.getControl node SkeletonControl))
794 ;; this is necessary to force the skeleton to have accurate world
795 ;; transforms before it is rendered to the screen.
796 (.resetAndUpdate)))
798 (defn green-x-ray []
799 (doto (Material. (asset-manager)
800 "Common/MatDefs/Misc/Unshaded.j3md")
801 (.setColor "Color" ColorRGBA/Green)
802 (-> (.getAdditionalRenderState)
803 (.setDepthTest false))))
805 (defn test-worm []
806 (.start
807 (world
808 (doto (Node.)
809 ;;(.attachChild (point-worm))
810 (.attachChild (load-blender-model
811 "Models/anim2/joint-worm.blend"))
813 (.attachChild (box 10 1 10
814 :position (Vector3f. 0 -2 0) :mass 0
815 :color (ColorRGBA/Gray))))
816 {
817 "key-space" (fire-cannon-ball)
818 }
819 (fn [world]
820 (enable-debug world)
821 (light-up-everything world)
822 ;;(.setTimer world (NanoTimer.))
823 )
824 no-op)))
828 ;; defunct movement stuff
829 (defn torque-controls [control]
830 (let [torques
831 (concat
832 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
833 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
834 [Vector3f/UNIT_X])]
835 (map (fn [torque-axis]
836 (fn [torque]
837 (.applyTorque
838 control
839 (.mult (.mult (.getPhysicsRotation control)
840 torque-axis)
841 (float
842 (* (.getMass control) torque))))))
843 torques)))
845 (defn motor-map
846 "Take a creature and generate a function that will enable fine
847 grained control over all the creature's limbs."
848 [#^Node creature]
849 (let [controls (keep #(.getControl % RigidBodyControl)
850 (node-seq creature))
851 limb-controls (reduce concat (map torque-controls controls))
852 body-control (partial map #(%1 %2) limb-controls)]
853 body-control))
855 (defn test-motor-map
856 "see how torque works."
857 []
858 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
859 :mass 1 :color ColorRGBA/Green)
860 motor-map (motor-map finger)]
861 (world
862 (nodify [finger
863 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
864 :color ColorRGBA/Gray)])
865 standard-debug-controls
866 (fn [world]
867 (set-gravity world Vector3f/ZERO)
868 (light-up-everything world)
869 (.setTimer world (NanoTimer.)))
870 (fn [_ _]
871 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
872 0]))))))
874 (defn joint-proprioception [#^Node parts #^Node joint]
875 (let [[obj-a obj-b] (joint-targets parts joint)
876 joint-rot (.getWorldRotation joint)
877 pre-inv-a (.inverse (.getWorldRotation obj-a))
878 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
879 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
880 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
882 x Vector3f/UNIT_Y
883 y Vector3f/UNIT_Z
884 z Vector3f/UNIT_X
887 tmp-rot-a (.getWorldRotation obj-a)]
888 (println-repl "x:" (.mult tmp-rot-a x))
889 (println-repl "y:" (.mult tmp-rot-a y))
890 (println-repl "z:" (.mult tmp-rot-a z))
891 (println-repl "rot-a" (.getWorldRotation obj-a))
892 (println-repl "rot-b" (.getWorldRotation obj-b))
893 (println-repl "joint-rot" joint-rot)
894 ;; this function will report proprioceptive information for the
895 ;; joint.
896 (fn []
897 ;; x is the "twist" axis, y and z are the "bend" axes
898 (let [rot-a (.getWorldRotation obj-a)
899 ;;inv-a (.inverse rot-a)
900 rot-b (.getWorldRotation obj-b)
901 ;;relative (.mult rot-b inv-a)
902 basis (doto (Matrix3f.)
903 (.setColumn 0 (.mult rot-a x))
904 (.setColumn 1 (.mult rot-a y))
905 (.setColumn 2 (.mult rot-a z)))
906 rotation-about-joint
907 (doto (Quaternion.)
908 (.fromRotationMatrix
909 (.mult (.invert basis)
910 (.toRotationMatrix rot-b))))
911 [yaw roll pitch]
912 (seq (.toAngles rotation-about-joint nil))]
913 ;;return euler angles of the quaternion around the new basis
914 [yaw roll pitch]))))
916 #+end_src
924 * COMMENT generate Source
925 #+begin_src clojure :tangle ../src/cortex/body.clj
926 <<proprioception>>
927 <<motor-control>>
928 #+end_src
930 #+begin_src clojure :tangle ../src/cortex/test/body.clj
931 <<test-body>>
932 #+end_src