view org/body.org @ 155:95bf55614211

moved goals and deleted old test
author Robert McIntyre <rlm@mit.edu>
date Fri, 03 Feb 2012 06:04:30 -0700
parents e1232043656a
children 84c67be00abe
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 right-handed? [vec1 vec2 vec3]
66 (< 0 (.dot (.cross vec1 vec2) vec3)))
68 (defn absolute-angle [vec1 vec2 axis]
69 (let [angle (.angleBetween vec1 vec2)]
70 (if (right-handed? vec1 vec2 axis)
71 angle (- (* 2 Math/PI) angle))))
74 (defn joint-proprioception [#^Node parts #^Node joint]
75 (let [[obj-a obj-b] (joint-targets parts joint)
76 joint-rot (.getWorldRotation joint)
77 x0 (.mult joint-rot Vector3f/UNIT_X)
78 y0 (.mult joint-rot Vector3f/UNIT_Y)
79 z0 (.mult joint-rot Vector3f/UNIT_Z)]
80 (println-repl "x:" x0)
81 (println-repl "y:" y0)
82 (println-repl "z:" z0)
83 (println-repl "init-a:" (.getWorldRotation obj-a))
84 (println-repl "init-b:" (.getWorldRotation obj-b))
86 (fn []
87 (let [rot-a (.clone (.getWorldRotation obj-a))
88 rot-b (.clone (.getWorldRotation obj-b))
89 x (.mult rot-a x0)
90 y (.mult rot-a y0)
91 z (.mult rot-a z0)
93 X (.mult rot-b x0)
94 Y (.mult rot-b y0)
95 Z (.mult rot-b z0)
96 heading (Math/atan2 (.dot X z) (.dot X x))
97 pitch (Math/atan2 (.dot X y) (.dot X x))
99 ;; rotate x-vector back to origin
100 reverse
101 (doto (Quaternion.)
102 (.fromAngleAxis
103 (.angleBetween X x)
104 (let [cross (.normalize (.cross X x))]
105 (if (= 0 (.length cross)) y cross))))
106 roll (absolute-angle (.mult reverse Y) y x)]
108 [heading pitch roll]))))
110 (defn proprioception
111 "Create a function that provides proprioceptive information about an
112 entire body."
113 [#^Node creature]
114 ;; extract the body's joints
115 (let [joints (creature-joints creature)
116 senses (map (partial joint-proprioception creature) joints)]
117 (fn []
118 (map #(%) senses))))
120 (defn tap [obj direction force]
121 (let [control (.getControl obj RigidBodyControl)]
122 (.applyTorque
123 control
124 (.mult (.getPhysicsRotation control)
125 (.mult (.normalize direction) (float force))))))
128 (defn with-movement
129 [object
130 [up down left right roll-up roll-down :as keyboard]
131 forces
132 [root-node
133 keymap
134 intilization
135 world-loop]]
136 (let [add-keypress
137 (fn [state keymap key]
138 (merge keymap
139 {key
140 (fn [_ pressed?]
141 (reset! state pressed?))}))
142 move-up? (atom false)
143 move-down? (atom false)
144 move-left? (atom false)
145 move-right? (atom false)
146 roll-left? (atom false)
147 roll-right? (atom false)
149 directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0)
150 (Vector3f. 0 0 1)(Vector3f. 0 0 -1)
151 (Vector3f. -1 0 0)(Vector3f. 1 0 0)]
152 atoms [move-left? move-right? move-up? move-down?
153 roll-left? roll-right?]
155 keymap* (reduce merge
156 (map #(add-keypress %1 keymap %2)
157 atoms
158 keyboard))
160 splice-loop (fn []
161 (dorun
162 (map
163 (fn [sym direction force]
164 (if @sym
165 (tap object direction force)))
166 atoms directions forces)))
168 world-loop* (fn [world tpf]
169 (world-loop world tpf)
170 (splice-loop))]
171 [root-node
172 keymap*
173 intilization
174 world-loop*]))
176 (import java.awt.image.BufferedImage)
178 (defn draw-sprite [image sprite x y color ]
179 (dorun
180 (for [[u v] sprite]
181 (.setRGB image (+ u x) (+ v y) color))))
183 (defn view-angle
184 "create a debug view of an angle"
185 [color]
186 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
187 previous (atom [25 25])
188 sprite [[0 0] [0 1]
189 [0 -1] [-1 0] [1 0]]]
190 (fn [angle]
191 (let [angle (float angle)]
192 (let [position
193 [(+ 25 (int (* 20 (Math/cos angle))))
194 (+ 25 (int (* -20 (Math/sin angle))))]]
195 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
196 (draw-sprite image sprite (position 0) (position 1) color)
197 (reset! previous position))
198 image))))
200 (defn proprioception-debug-window
201 []
202 (let [heading (view-angle 0xFF0000)
203 pitch (view-angle 0x00FF00)
204 roll (view-angle 0xFFFFFF)
205 v-heading (view-image)
206 v-pitch (view-image)
207 v-roll (view-image)
208 ]
209 (fn [prop-data]
210 (dorun
211 (map
212 (fn [[h p r]]
213 (v-heading (heading h))
214 (v-pitch (pitch p))
215 (v-roll (roll r)))
216 prop-data)))))
219 #+end_src
221 #+results: proprioception
222 : #'cortex.body/proprioception-debug-window
224 * Motor Control
225 #+name: motor-control
226 #+begin_src clojure
227 (in-ns 'cortex.body)
229 ;; surprisingly enough, terristerial creatures only move by using
230 ;; torque applied about their joints. There's not a single straight
231 ;; line of force in the human body at all! (A straight line of force
232 ;; would correspond to some sort of jet or rocket propulseion.)
234 (defn vector-motor-control
235 "Create a function that accepts a sequence of Vector3f objects that
236 describe the torque to be applied to each part of the body."
237 [body]
238 (let [nodes (node-seq body)
239 controls (keep #(.getControl % RigidBodyControl) nodes)]
240 (fn [torques]
241 (map #(.applyTorque %1 %2)
242 controls torques))))
243 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
244 #+end_src
246 ## note -- might want to add a lower dimensional, discrete version of
247 ## this if it proves useful from a x-modal clustering perspective.
249 * Examples
251 #+name: test-body
252 #+begin_src clojure
253 (ns cortex.test.body
254 (:use (cortex world util body))
255 (:require cortex.silly)
256 (:import
257 com.jme3.math.Vector3f
258 com.jme3.math.ColorRGBA
259 com.jme3.bullet.joints.Point2PointJoint
260 com.jme3.bullet.control.RigidBodyControl
261 com.jme3.system.NanoTimer
262 com.jme3.math.Quaternion))
264 (defn worm-segments
265 "Create multiple evenly spaced box segments. They're fabulous!"
266 [segment-length num-segments interstitial-space radius]
267 (letfn [(nth-segment
268 [n]
269 (box segment-length radius radius :mass 0.1
270 :position
271 (Vector3f.
272 (* 2 n (+ interstitial-space segment-length)) 0 0)
273 :name (str "worm-segment" n)
274 :color (ColorRGBA/randomColor)))]
275 (map nth-segment (range num-segments))))
277 (defn connect-at-midpoint
278 "Connect two physics objects with a Point2Point joint constraint at
279 the point equidistant from both objects' centers."
280 [segmentA segmentB]
281 (let [centerA (.getWorldTranslation segmentA)
282 centerB (.getWorldTranslation segmentB)
283 midpoint (.mult (.add centerA centerB) (float 0.5))
284 pivotA (.subtract midpoint centerA)
285 pivotB (.subtract midpoint centerB)
287 ;; A side-effect of creating a joint registers
288 ;; it with both physics objects which in turn
289 ;; will register the joint with the physics system
290 ;; when the simulation is started.
291 joint (Point2PointJoint.
292 (.getControl segmentA RigidBodyControl)
293 (.getControl segmentB RigidBodyControl)
294 pivotA
295 pivotB)]
296 segmentB))
298 (defn eve-worm
299 "Create a worm-like body bound by invisible joint constraints."
300 []
301 (let [segments (worm-segments 0.2 5 0.1 0.1)]
302 (dorun (map (partial apply connect-at-midpoint)
303 (partition 2 1 segments)))
304 (nodify "worm" segments)))
306 (defn worm-pattern
307 "This is a simple, mindless motor control pattern that drives the
308 second segment of the worm's body at an offset angle with
309 sinusoidally varying strength."
310 [time]
311 (let [angle (* Math/PI (/ 9 20))
312 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
313 [Vector3f/ZERO
314 (.mult
315 direction
316 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
317 Vector3f/ZERO
318 Vector3f/ZERO
319 Vector3f/ZERO]))
321 (defn test-motor-control
322 "Testing motor-control:
323 You should see a multi-segmented worm-like object fall onto the
324 table and begin writhing and moving."
325 []
326 (let [worm (eve-worm)
327 time (atom 0)
328 worm-motor-map (vector-motor-control worm)]
329 (world
330 (nodify [worm
331 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
332 :color ColorRGBA/Gray)])
333 standard-debug-controls
334 (fn [world]
335 (enable-debug world)
336 (light-up-everything world)
337 (comment
338 (com.aurellem.capture.Capture/captureVideo
339 world
340 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
341 )
343 (fn [_ _]
344 (swap! time inc)
345 (Thread/sleep 20)
346 (dorun (worm-motor-map
347 (worm-pattern @time)))))))
351 (defn join-at-point [obj-a obj-b world-pivot]
352 (cortex.silly/joint-dispatch
353 {:type :point}
354 (.getControl obj-a RigidBodyControl)
355 (.getControl obj-b RigidBodyControl)
356 (cortex.silly/world-to-local obj-a world-pivot)
357 (cortex.silly/world-to-local obj-b world-pivot)
358 nil
359 ))
361 (import com.jme3.bullet.collision.PhysicsCollisionObject)
363 (defn blab-* []
364 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
365 :mass 0 :color ColorRGBA/Green)
366 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
367 :mass 1 :color ColorRGBA/Red)
368 connection-point (Vector3f. 1.2 0 0)
369 root (nodify [hand finger])]
371 (join-at-point hand finger (Vector3f. 1.2 0 0))
373 (.setCollisionGroup
374 (.getControl hand RigidBodyControl)
375 PhysicsCollisionObject/COLLISION_GROUP_NONE)
376 (world
377 root
378 standard-debug-controls
379 (fn [world]
380 (enable-debug world)
381 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
382 (set-gravity world Vector3f/ZERO)
383 )
384 no-op)))
385 (comment
387 (defn proprioception-debug-window
388 []
389 (let [time (atom 0)]
390 (fn [prop-data]
391 (if (= 0 (rem (swap! time inc) 40))
392 (println-repl prop-data)))))
393 )
395 (comment
396 (dorun
397 (map
398 (comp
399 println-repl
400 (fn [[p y r]]
401 (format
402 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
403 p y r)))
404 prop-data)))
409 (defn test-proprioception
410 "Testing proprioception:
411 You should see two foating bars, and a printout of pitch, yaw, and
412 roll. Pressing key-r/key-t should move the blue bar up and down and
413 change only the value of pitch. key-f/key-g moves it side to side
414 and changes yaw. key-v/key-b will spin the blue segment clockwise
415 and counterclockwise, and only affect roll."
416 []
417 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
418 :mass 0 :color ColorRGBA/Green :name "hand")
419 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
420 :mass 1 :color ColorRGBA/Red :name "finger")
421 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
422 :position (Vector3f. 0 1.2 0)
423 :rotation (doto (Quaternion.)
424 (.fromAngleAxis
425 (/ Math/PI 2)
426 (Vector3f. 0 0 1)))
427 :physical? false)
428 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
429 creature (nodify [hand finger joint-node])
430 finger-control (.getControl finger RigidBodyControl)
431 hand-control (.getControl hand RigidBodyControl)]
434 (let
435 ;; *******************************************
437 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
438 :mass 0 :color ColorRGBA/Gray)
440 root (nodify [creature floor])
441 prop (joint-proprioception creature joint-node)
442 prop-view (proprioception-debug-window)
444 controls
445 (merge standard-debug-controls
446 {"key-o"
447 (fn [_ _] (.setEnabled finger-control true))
448 "key-p"
449 (fn [_ _] (.setEnabled finger-control false))
450 "key-k"
451 (fn [_ _] (.setEnabled hand-control true))
452 "key-l"
453 (fn [_ _] (.setEnabled hand-control false))
454 "key-i"
455 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
456 "key-period"
457 (fn [world _]
458 (.setEnabled finger-control false)
459 (.setEnabled hand-control false)
460 (.rotate creature (doto (Quaternion.)
461 (.fromAngleAxis
462 (float (/ Math/PI 15))
463 (Vector3f. 0 0 -1))))
465 (.setEnabled finger-control true)
466 (.setEnabled hand-control true)
467 (set-gravity world (Vector3f. 0 0 0))
468 )
471 }
472 )
474 ]
475 (comment
476 (.setCollisionGroup
477 (.getControl hand RigidBodyControl)
478 PhysicsCollisionObject/COLLISION_GROUP_NONE)
479 )
480 (apply
481 world
482 (with-movement
483 hand
484 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
485 [10 10 10 10 1 1]
486 (with-movement
487 finger
488 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
489 [1 1 10 10 10 10]
490 [root
491 controls
492 (fn [world]
493 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
494 (set-gravity world (Vector3f. 0 0 0))
495 (light-up-everything world))
496 (fn [_ _] (prop-view (list (prop))))]))))))
498 #+end_src
500 #+results: test-body
501 : #'cortex.test.body/test-proprioception
504 * COMMENT code-limbo
505 #+begin_src clojure
506 ;;(.loadModel
507 ;; (doto (asset-manager)
508 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
509 ;; "Models/person/person.blend")
512 (defn load-blender-model
513 "Load a .blend file using an asset folder relative path."
514 [^String model]
515 (.loadModel
516 (doto (asset-manager)
517 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
518 model))
521 (defn view-model [^String model]
522 (view
523 (.loadModel
524 (doto (asset-manager)
525 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
526 model)))
528 (defn load-blender-scene [^String model]
529 (.loadModel
530 (doto (asset-manager)
531 (.registerLoader BlenderLoader (into-array String ["blend"])))
532 model))
534 (defn worm
535 []
536 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
538 (defn oto
539 []
540 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
542 (defn sinbad
543 []
544 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
546 (defn worm-blender
547 []
548 (first (seq (.getChildren (load-blender-model
549 "Models/anim2/simple-worm.blend")))))
551 (defn body
552 "given a node with a SkeletonControl, will produce a body sutiable
553 for AI control with movement and proprioception."
554 [node]
555 (let [skeleton-control (.getControl node SkeletonControl)
556 krc (KinematicRagdollControl.)]
557 (comment
558 (dorun
559 (map #(.addBoneName krc %)
560 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
561 ;;"mid2" "mid3" "tail" "head"]
562 )))
563 (.addControl node krc)
564 (.setRagdollMode krc)
565 )
566 node
567 )
568 (defn show-skeleton [node]
569 (let [sd
571 (doto
572 (SkeletonDebugger. "aurellem-skel-debug"
573 (skel node))
574 (.setMaterial (green-x-ray)))]
575 (.attachChild node sd)
576 node))
580 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
582 ;; this could be a good way to give objects special properties like
583 ;; being eyes and the like
585 (.getUserData
586 (.getChild
587 (load-blender-model "Models/property/test.blend") 0)
588 "properties")
590 ;; the properties are saved along with the blender file.
591 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
596 (defn init-debug-skel-node
597 [f debug-node skeleton]
598 (let [bones
599 (map #(.getBone skeleton %)
600 (range (.getBoneCount skeleton)))]
601 (dorun (map #(.setUserControl % true) bones))
602 (dorun (map (fn [b]
603 (println (.getName b)
604 " -- " (f b)))
605 bones))
606 (dorun
607 (map #(.attachChild
608 debug-node
609 (doto
610 (sphere 0.1
611 :position (f %)
612 :physical? false)
613 (.setMaterial (green-x-ray))))
614 bones)))
615 debug-node)
617 (import jme3test.bullet.PhysicsTestHelper)
620 (defn test-zzz [the-worm world value]
621 (if (not value)
622 (let [skeleton (skel the-worm)]
623 (println-repl "enabling bones")
624 (dorun
625 (map
626 #(.setUserControl (.getBone skeleton %) true)
627 (range (.getBoneCount skeleton))))
630 (let [b (.getBone skeleton 2)]
631 (println-repl "moving " (.getName b))
632 (println-repl (.getLocalPosition b))
633 (.setUserTransforms b
634 Vector3f/UNIT_X
635 Quaternion/IDENTITY
636 ;;(doto (Quaternion.)
637 ;; (.fromAngles (/ Math/PI 2)
638 ;; 0
639 ;; 0
641 (Vector3f. 1 1 1))
642 )
644 (println-repl "hi! <3"))))
647 (defn test-ragdoll []
649 (let [the-worm
651 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
652 (doto (show-skeleton (worm-blender))
653 (.setLocalTranslation (Vector3f. 0 10 0))
654 ;;(worm)
655 ;;(oto)
656 ;;(sinbad)
657 )
658 ]
661 (.start
662 (world
663 (doto (Node.)
664 (.attachChild the-worm))
665 {"key-return" (fire-cannon-ball)
666 "key-space" (partial test-zzz the-worm)
667 }
668 (fn [world]
669 (light-up-everything world)
670 (PhysicsTestHelper/createPhysicsTestWorld
671 (.getRootNode world)
672 (asset-manager)
673 (.getPhysicsSpace
674 (.getState (.getStateManager world) BulletAppState)))
675 (set-gravity world Vector3f/ZERO)
676 ;;(.setTimer world (NanoTimer.))
677 ;;(org.lwjgl.input.Mouse/setGrabbed false)
678 )
679 no-op
680 )
683 )))
686 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
687 ;;; here is the ragdoll stuff
689 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
690 (def mesh worm-mesh)
692 (.getFloatBuffer mesh VertexBuffer$Type/Position)
693 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
694 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
697 (defn position [index]
698 (.get
699 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
700 index))
702 (defn bones [index]
703 (.get
704 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
705 index))
707 (defn bone-weights [index]
708 (.get
709 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
710 index))
714 (defn vertex-bones [vertex]
715 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
717 (defn vertex-weights [vertex]
718 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
720 (defn vertex-position [index]
721 (let [offset (* index 3)]
722 (Vector3f. (position offset)
723 (position (inc offset))
724 (position (inc(inc offset))))))
726 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
728 (defn bone-control-color [index]
729 (get {[1 0 0 0] ColorRGBA/Red
730 [1 2 0 0] ColorRGBA/Magenta
731 [2 0 0 0] ColorRGBA/Blue}
732 (vertex-bones index)
733 ColorRGBA/White))
735 (defn influence-color [index bone-num]
736 (get
737 {(float 0) ColorRGBA/Blue
738 (float 0.5) ColorRGBA/Green
739 (float 1) ColorRGBA/Red}
740 ;; find the weight of the desired bone
741 ((zipmap (vertex-bones index)(vertex-weights index))
742 bone-num)
743 ColorRGBA/Blue))
745 (def worm-vertices (set (map vertex-info (range 60))))
748 (defn test-info []
749 (let [points (Node.)]
750 (dorun
751 (map #(.attachChild points %)
752 (map #(sphere 0.01
753 :position (vertex-position %)
754 :color (influence-color % 1)
755 :physical? false)
756 (range 60))))
757 (view points)))
760 (defrecord JointControl [joint physics-space]
761 PhysicsControl
762 (setPhysicsSpace [this space]
763 (dosync
764 (ref-set (:physics-space this) space))
765 (.addJoint space (:joint this)))
766 (update [this tpf])
767 (setSpatial [this spatial])
768 (render [this rm vp])
769 (getPhysicsSpace [this] (deref (:physics-space this)))
770 (isEnabled [this] true)
771 (setEnabled [this state]))
773 (defn add-joint
774 "Add a joint to a particular object. When the object is added to the
775 PhysicsSpace of a simulation, the joint will also be added"
776 [object joint]
777 (let [control (JointControl. joint (ref nil))]
778 (.addControl object control))
779 object)
782 (defn hinge-world
783 []
784 (let [sphere1 (sphere)
785 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
786 joint (Point2PointJoint.
787 (.getControl sphere1 RigidBodyControl)
788 (.getControl sphere2 RigidBodyControl)
789 Vector3f/ZERO (Vector3f. 3 3 3))]
790 (add-joint sphere1 joint)
791 (doto (Node. "hinge-world")
792 (.attachChild sphere1)
793 (.attachChild sphere2))))
796 (defn test-joint []
797 (view (hinge-world)))
799 ;; (defn copier-gen []
800 ;; (let [count (atom 0)]
801 ;; (fn [in]
802 ;; (swap! count inc)
803 ;; (clojure.contrib.duck-streams/copy
804 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
805 ;; ;;/home/r/tmp/mao-test/clojure-images
806 ;; (format "%08d.png" @count)))))))
807 ;; (defn decrease-framerate []
808 ;; (map
809 ;; (copier-gen)
810 ;; (sort
811 ;; (map first
812 ;; (partition
813 ;; 4
814 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
815 ;; (file-seq
816 ;; (file-str
817 ;; "/home/r/media/anime/mao-temp/images"))))))))
821 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
823 (defn proprioception
824 "Create a proprioception map that reports the rotations of the
825 various limbs of the creature's body"
826 [creature]
827 [#^Node creature]
828 (let [
829 nodes (node-seq creature)
830 joints
831 (map
832 :joint
833 (filter
834 #(isa? (class %) JointControl)
835 (reduce
836 concat
837 (map (fn [node]
838 (map (fn [num] (.getControl node num))
839 (range (.getNumControls node))))
840 nodes))))]
841 (fn []
842 (reduce concat (map relative-positions (list (first joints)))))))
845 (defn skel [node]
846 (doto
847 (.getSkeleton
848 (.getControl node SkeletonControl))
849 ;; this is necessary to force the skeleton to have accurate world
850 ;; transforms before it is rendered to the screen.
851 (.resetAndUpdate)))
853 (defn green-x-ray []
854 (doto (Material. (asset-manager)
855 "Common/MatDefs/Misc/Unshaded.j3md")
856 (.setColor "Color" ColorRGBA/Green)
857 (-> (.getAdditionalRenderState)
858 (.setDepthTest false))))
860 (defn test-worm []
861 (.start
862 (world
863 (doto (Node.)
864 ;;(.attachChild (point-worm))
865 (.attachChild (load-blender-model
866 "Models/anim2/joint-worm.blend"))
868 (.attachChild (box 10 1 10
869 :position (Vector3f. 0 -2 0) :mass 0
870 :color (ColorRGBA/Gray))))
871 {
872 "key-space" (fire-cannon-ball)
873 }
874 (fn [world]
875 (enable-debug world)
876 (light-up-everything world)
877 ;;(.setTimer world (NanoTimer.))
878 )
879 no-op)))
883 ;; defunct movement stuff
884 (defn torque-controls [control]
885 (let [torques
886 (concat
887 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
888 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
889 [Vector3f/UNIT_X])]
890 (map (fn [torque-axis]
891 (fn [torque]
892 (.applyTorque
893 control
894 (.mult (.mult (.getPhysicsRotation control)
895 torque-axis)
896 (float
897 (* (.getMass control) torque))))))
898 torques)))
900 (defn motor-map
901 "Take a creature and generate a function that will enable fine
902 grained control over all the creature's limbs."
903 [#^Node creature]
904 (let [controls (keep #(.getControl % RigidBodyControl)
905 (node-seq creature))
906 limb-controls (reduce concat (map torque-controls controls))
907 body-control (partial map #(%1 %2) limb-controls)]
908 body-control))
910 (defn test-motor-map
911 "see how torque works."
912 []
913 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
914 :mass 1 :color ColorRGBA/Green)
915 motor-map (motor-map finger)]
916 (world
917 (nodify [finger
918 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
919 :color ColorRGBA/Gray)])
920 standard-debug-controls
921 (fn [world]
922 (set-gravity world Vector3f/ZERO)
923 (light-up-everything world)
924 (.setTimer world (NanoTimer.)))
925 (fn [_ _]
926 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
927 0]))))))
929 (defn joint-proprioception [#^Node parts #^Node joint]
930 (let [[obj-a obj-b] (joint-targets parts joint)
931 joint-rot (.getWorldRotation joint)
932 pre-inv-a (.inverse (.getWorldRotation obj-a))
933 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
934 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
935 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
937 x Vector3f/UNIT_Y
938 y Vector3f/UNIT_Z
939 z Vector3f/UNIT_X
942 tmp-rot-a (.getWorldRotation obj-a)]
943 (println-repl "x:" (.mult tmp-rot-a x))
944 (println-repl "y:" (.mult tmp-rot-a y))
945 (println-repl "z:" (.mult tmp-rot-a z))
946 (println-repl "rot-a" (.getWorldRotation obj-a))
947 (println-repl "rot-b" (.getWorldRotation obj-b))
948 (println-repl "joint-rot" joint-rot)
949 ;; this function will report proprioceptive information for the
950 ;; joint.
951 (fn []
952 ;; x is the "twist" axis, y and z are the "bend" axes
953 (let [rot-a (.getWorldRotation obj-a)
954 ;;inv-a (.inverse rot-a)
955 rot-b (.getWorldRotation obj-b)
956 ;;relative (.mult rot-b inv-a)
957 basis (doto (Matrix3f.)
958 (.setColumn 0 (.mult rot-a x))
959 (.setColumn 1 (.mult rot-a y))
960 (.setColumn 2 (.mult rot-a z)))
961 rotation-about-joint
962 (doto (Quaternion.)
963 (.fromRotationMatrix
964 (.mult (.invert basis)
965 (.toRotationMatrix rot-b))))
966 [yaw roll pitch]
967 (seq (.toAngles rotation-about-joint nil))]
968 ;;return euler angles of the quaternion around the new basis
969 [yaw roll pitch]))))
971 #+end_src
979 * COMMENT generate Source
980 #+begin_src clojure :tangle ../src/cortex/body.clj
981 <<proprioception>>
982 <<motor-control>>
983 #+end_src
985 #+begin_src clojure :tangle ../src/cortex/test/body.clj
986 <<test-body>>
987 #+end_src