view org/body.org @ 149:1e6beed24cec

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