view org/body.org @ 171:15bde60217aa

updated docstring for vision-fn
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 05:02:25 -0700
parents 33278bf028e7
children 0b9ae09eaec3
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 sense))
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 (cortex.import/mega-import-jme3)
25 (defn jme-to-blender
26 "Convert from JME coordinates to Blender coordinates"
27 [#^Vector3f in]
28 (Vector3f. (.getX in)
29 (- (.getZ in))
30 (.getY in)))
32 (defn blender-to-jme
33 "Convert from Blender coordinates to JME coordinates"
34 [#^Vector3f in]
35 (Vector3f. (.getX in)
36 (.getZ in)
37 (- (.getY in))))
40 (defn joint-targets
41 "Return the two closest two objects to the joint object, ordered
42 from bottom to top according to the joint's rotation."
43 [#^Node parts #^Node joint]
44 (loop [radius (float 0.01)]
45 (let [results (CollisionResults.)]
46 (.collideWith
47 parts
48 (BoundingBox. (.getWorldTranslation joint)
49 radius radius radius)
50 results)
51 (let [targets
52 (distinct
53 (map #(.getGeometry %) results))]
54 (if (>= (count targets) 2)
55 (sort-by
56 #(let [v
57 (jme-to-blender
58 (.mult
59 (.inverse (.getWorldRotation joint))
60 (.subtract (.getWorldTranslation %)
61 (.getWorldTranslation joint))))]
62 (println-repl (.getName %) ":" v)
63 (.dot (Vector3f. 1 1 1)
64 v))
65 (take 2 targets))
66 (recur (float (* radius 2))))))))
68 (defn creature-joints
69 "Return the children of the creature's \"joints\" node."
70 [#^Node creature]
71 (if-let [joint-node (.getChild creature "joints")]
72 (seq (.getChildren joint-node))
73 (do (println-repl "could not find JOINTS node") [])))
75 (defmulti joint-dispatch
76 "Translate blender pseudo-joints into real JME joints."
77 (fn [constraints & _]
78 (:type constraints)))
80 (defmethod joint-dispatch :point
81 [constraints control-a control-b pivot-a pivot-b rotation]
82 (println-repl "creating POINT2POINT joint")
83 ;; bullet's point2point joints are BROKEN, so we must use the
84 ;; generic 6DOF joint instead of an actual Point2Point joint!
86 ;; should be able to do this:
87 (comment
88 (Point2PointJoint.
89 control-a
90 control-b
91 pivot-a
92 pivot-b))
94 ;; but instead we must do this:
95 (println-repl "substuting 6DOF joint for POINT2POINT joint!")
96 (doto
97 (SixDofJoint.
98 control-a
99 control-b
100 pivot-a
101 pivot-b
102 false)
103 (.setLinearLowerLimit Vector3f/ZERO)
104 (.setLinearUpperLimit Vector3f/ZERO)
105 ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
106 ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
108 ))
111 (defmethod joint-dispatch :hinge
112 [constraints control-a control-b pivot-a pivot-b rotation]
113 (println-repl "creating HINGE joint")
114 (let [axis
115 (if-let
116 [axis (:axis constraints)]
117 axis
118 Vector3f/UNIT_X)
119 [limit-1 limit-2] (:limit constraints)
120 hinge-axis
121 (.mult
122 rotation
123 (blender-to-jme axis))]
124 (doto
125 (HingeJoint.
126 control-a
127 control-b
128 pivot-a
129 pivot-b
130 hinge-axis
131 hinge-axis)
132 (.setLimit limit-1 limit-2))))
134 (defmethod joint-dispatch :cone
135 [constraints control-a control-b pivot-a pivot-b rotation]
136 (let [limit-xz (:limit-xz constraints)
137 limit-xy (:limit-xy constraints)
138 twist (:twist constraints)]
140 (println-repl "creating CONE joint")
141 (println-repl rotation)
142 (println-repl
143 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
144 (println-repl
145 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
146 (println-repl
147 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
148 (doto
149 (ConeJoint.
150 control-a
151 control-b
152 pivot-a
153 pivot-b
154 rotation
155 rotation)
156 (.setLimit (float limit-xz)
157 (float limit-xy)
158 (float twist)))))
160 (defn connect
161 "here are some examples:
162 {:type :point}
163 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
164 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
166 {:type :cone :limit-xz 0]
167 :limit-xy 0]
168 :twist 0]} (use XZY rotation mode in blender!)"
169 [#^Node obj-a #^Node obj-b #^Node joint]
170 (let [control-a (.getControl obj-a RigidBodyControl)
171 control-b (.getControl obj-b RigidBodyControl)
172 joint-center (.getWorldTranslation joint)
173 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
174 pivot-a (world-to-local obj-a joint-center)
175 pivot-b (world-to-local obj-b joint-center)]
177 (if-let [constraints
178 (map-vals
179 eval
180 (read-string
181 (meta-data joint "joint")))]
182 ;; A side-effect of creating a joint registers
183 ;; it with both physics objects which in turn
184 ;; will register the joint with the physics system
185 ;; when the simulation is started.
186 (do
187 (println-repl "creating joint between"
188 (.getName obj-a) "and" (.getName obj-b))
189 (joint-dispatch constraints
190 control-a control-b
191 pivot-a pivot-b
192 joint-rotation))
193 (println-repl "could not find joint meta-data!"))))
198 (defn assemble-creature [#^Node pieces joints]
199 (dorun
200 (map
201 (fn [geom]
202 (let [physics-control
203 (RigidBodyControl.
204 (HullCollisionShape.
205 (.getMesh geom))
206 (if-let [mass (meta-data geom "mass")]
207 (do
208 (println-repl
209 "setting" (.getName geom) "mass to" (float mass))
210 (float mass))
211 (float 1)))]
213 (.addControl geom physics-control)))
214 (filter #(isa? (class %) Geometry )
215 (node-seq pieces))))
216 (dorun
217 (map
218 (fn [joint]
219 (let [[obj-a obj-b] (joint-targets pieces joint)]
220 (connect obj-a obj-b joint)))
221 joints))
222 pieces)
225 ;; TODO implement a function that adds joints in the style of the
226 ;; other sense functions
230 (import java.awt.image.BufferedImage)
232 (defn draw-sprite [image sprite x y color ]
233 (dorun
234 (for [[u v] sprite]
235 (.setRGB image (+ u x) (+ v y) color))))
237 (defn view-angle
238 "create a debug view of an angle"
239 [color]
240 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
241 previous (atom [25 25])
242 sprite [[0 0] [0 1]
243 [0 -1] [-1 0] [1 0]]]
244 (fn [angle]
245 (let [angle (float angle)]
246 (let [position
247 [(+ 25 (int (* 20 (Math/cos angle))))
248 (+ 25 (int (* -20 (Math/sin angle))))]]
249 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
250 (draw-sprite image sprite (position 0) (position 1) color)
251 (reset! previous position))
252 image))))
254 (defn proprioception-debug-window
255 []
256 (let [heading (view-angle 0xFF0000)
257 pitch (view-angle 0x00FF00)
258 roll (view-angle 0xFFFFFF)
259 v-heading (view-image)
260 v-pitch (view-image)
261 v-roll (view-image)
262 ]
263 (fn [prop-data]
264 (dorun
265 (map
266 (fn [[h p r]]
267 (v-heading (heading h))
268 (v-pitch (pitch p))
269 (v-roll (roll r)))
270 prop-data)))))
273 #+end_src
275 #+results: proprioception
276 : #'cortex.body/proprioception-debug-window
278 * Motor Control
279 #+name: motor-control
280 #+begin_src clojure
281 (in-ns 'cortex.body)
283 ;; surprisingly enough, terristerial creatures only move by using
284 ;; torque applied about their joints. There's not a single straight
285 ;; line of force in the human body at all! (A straight line of force
286 ;; would correspond to some sort of jet or rocket propulseion.)
288 (defn vector-motor-control
289 "Create a function that accepts a sequence of Vector3f objects that
290 describe the torque to be applied to each part of the body."
291 [body]
292 (let [nodes (node-seq body)
293 controls (keep #(.getControl % RigidBodyControl) nodes)]
294 (fn [torques]
295 (map #(.applyTorque %1 %2)
296 controls torques))))
297 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
298 #+end_src
300 ## note -- might want to add a lower dimensional, discrete version of
301 ## this if it proves useful from a x-modal clustering perspective.
303 * Examples
305 #+name: test-body
306 #+begin_src clojure
307 (ns cortex.test.body
308 (:use (cortex world util body))
309 (:require cortex.silly)
310 (:import
311 com.jme3.math.Vector3f
312 com.jme3.math.ColorRGBA
313 com.jme3.bullet.joints.Point2PointJoint
314 com.jme3.bullet.control.RigidBodyControl
315 com.jme3.system.NanoTimer
316 com.jme3.math.Quaternion))
318 (defn worm-segments
319 "Create multiple evenly spaced box segments. They're fabulous!"
320 [segment-length num-segments interstitial-space radius]
321 (letfn [(nth-segment
322 [n]
323 (box segment-length radius radius :mass 0.1
324 :position
325 (Vector3f.
326 (* 2 n (+ interstitial-space segment-length)) 0 0)
327 :name (str "worm-segment" n)
328 :color (ColorRGBA/randomColor)))]
329 (map nth-segment (range num-segments))))
331 (defn connect-at-midpoint
332 "Connect two physics objects with a Point2Point joint constraint at
333 the point equidistant from both objects' centers."
334 [segmentA segmentB]
335 (let [centerA (.getWorldTranslation segmentA)
336 centerB (.getWorldTranslation segmentB)
337 midpoint (.mult (.add centerA centerB) (float 0.5))
338 pivotA (.subtract midpoint centerA)
339 pivotB (.subtract midpoint centerB)
341 ;; A side-effect of creating a joint registers
342 ;; it with both physics objects which in turn
343 ;; will register the joint with the physics system
344 ;; when the simulation is started.
345 joint (Point2PointJoint.
346 (.getControl segmentA RigidBodyControl)
347 (.getControl segmentB RigidBodyControl)
348 pivotA
349 pivotB)]
350 segmentB))
352 (defn eve-worm
353 "Create a worm-like body bound by invisible joint constraints."
354 []
355 (let [segments (worm-segments 0.2 5 0.1 0.1)]
356 (dorun (map (partial apply connect-at-midpoint)
357 (partition 2 1 segments)))
358 (nodify "worm" segments)))
360 (defn worm-pattern
361 "This is a simple, mindless motor control pattern that drives the
362 second segment of the worm's body at an offset angle with
363 sinusoidally varying strength."
364 [time]
365 (let [angle (* Math/PI (/ 9 20))
366 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
367 [Vector3f/ZERO
368 (.mult
369 direction
370 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
371 Vector3f/ZERO
372 Vector3f/ZERO
373 Vector3f/ZERO]))
375 (defn test-motor-control
376 "Testing motor-control:
377 You should see a multi-segmented worm-like object fall onto the
378 table and begin writhing and moving."
379 []
380 (let [worm (eve-worm)
381 time (atom 0)
382 worm-motor-map (vector-motor-control worm)]
383 (world
384 (nodify [worm
385 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
386 :color ColorRGBA/Gray)])
387 standard-debug-controls
388 (fn [world]
389 (enable-debug world)
390 (light-up-everything world)
391 (comment
392 (com.aurellem.capture.Capture/captureVideo
393 world
394 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
395 )
397 (fn [_ _]
398 (swap! time inc)
399 (Thread/sleep 20)
400 (dorun (worm-motor-map
401 (worm-pattern @time)))))))
405 (defn join-at-point [obj-a obj-b world-pivot]
406 (cortex.silly/joint-dispatch
407 {:type :point}
408 (.getControl obj-a RigidBodyControl)
409 (.getControl obj-b RigidBodyControl)
410 (cortex.silly/world-to-local obj-a world-pivot)
411 (cortex.silly/world-to-local obj-b world-pivot)
412 nil
413 ))
415 (import com.jme3.bullet.collision.PhysicsCollisionObject)
417 (defn blab-* []
418 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
419 :mass 0 :color ColorRGBA/Green)
420 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
421 :mass 1 :color ColorRGBA/Red)
422 connection-point (Vector3f. 1.2 0 0)
423 root (nodify [hand finger])]
425 (join-at-point hand finger (Vector3f. 1.2 0 0))
427 (.setCollisionGroup
428 (.getControl hand RigidBodyControl)
429 PhysicsCollisionObject/COLLISION_GROUP_NONE)
430 (world
431 root
432 standard-debug-controls
433 (fn [world]
434 (enable-debug world)
435 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
436 (set-gravity world Vector3f/ZERO)
437 )
438 no-op)))
439 (comment
441 (defn proprioception-debug-window
442 []
443 (let [time (atom 0)]
444 (fn [prop-data]
445 (if (= 0 (rem (swap! time inc) 40))
446 (println-repl prop-data)))))
447 )
449 (comment
450 (dorun
451 (map
452 (comp
453 println-repl
454 (fn [[p y r]]
455 (format
456 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
457 p y r)))
458 prop-data)))
463 (defn test-proprioception
464 "Testing proprioception:
465 You should see two foating bars, and a printout of pitch, yaw, and
466 roll. Pressing key-r/key-t should move the blue bar up and down and
467 change only the value of pitch. key-f/key-g moves it side to side
468 and changes yaw. key-v/key-b will spin the blue segment clockwise
469 and counterclockwise, and only affect roll."
470 []
471 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
472 :mass 0 :color ColorRGBA/Green :name "hand")
473 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
474 :mass 1 :color ColorRGBA/Red :name "finger")
475 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
476 :position (Vector3f. 0 1.2 0)
477 :rotation (doto (Quaternion.)
478 (.fromAngleAxis
479 (/ Math/PI 2)
480 (Vector3f. 0 0 1)))
481 :physical? false)
482 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
483 creature (nodify [hand finger joint-node])
484 finger-control (.getControl finger RigidBodyControl)
485 hand-control (.getControl hand RigidBodyControl)]
488 (let
489 ;; *******************************************
491 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
492 :mass 0 :color ColorRGBA/Gray)
494 root (nodify [creature floor])
495 prop (joint-proprioception creature joint-node)
496 prop-view (proprioception-debug-window)
498 controls
499 (merge standard-debug-controls
500 {"key-o"
501 (fn [_ _] (.setEnabled finger-control true))
502 "key-p"
503 (fn [_ _] (.setEnabled finger-control false))
504 "key-k"
505 (fn [_ _] (.setEnabled hand-control true))
506 "key-l"
507 (fn [_ _] (.setEnabled hand-control false))
508 "key-i"
509 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
510 "key-period"
511 (fn [world _]
512 (.setEnabled finger-control false)
513 (.setEnabled hand-control false)
514 (.rotate creature (doto (Quaternion.)
515 (.fromAngleAxis
516 (float (/ Math/PI 15))
517 (Vector3f. 0 0 -1))))
519 (.setEnabled finger-control true)
520 (.setEnabled hand-control true)
521 (set-gravity world (Vector3f. 0 0 0))
522 )
525 }
526 )
528 ]
529 (comment
530 (.setCollisionGroup
531 (.getControl hand RigidBodyControl)
532 PhysicsCollisionObject/COLLISION_GROUP_NONE)
533 )
534 (apply
535 world
536 (with-movement
537 hand
538 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
539 [10 10 10 10 1 1]
540 (with-movement
541 finger
542 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
543 [1 1 10 10 10 10]
544 [root
545 controls
546 (fn [world]
547 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
548 (set-gravity world (Vector3f. 0 0 0))
549 (light-up-everything world))
550 (fn [_ _] (prop-view (list (prop))))]))))))
552 #+end_src
554 #+results: test-body
555 : #'cortex.test.body/test-proprioception
558 * COMMENT code-limbo
559 #+begin_src clojure
560 ;;(.loadModel
561 ;; (doto (asset-manager)
562 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
563 ;; "Models/person/person.blend")
566 (defn load-blender-model
567 "Load a .blend file using an asset folder relative path."
568 [^String model]
569 (.loadModel
570 (doto (asset-manager)
571 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
572 model))
575 (defn view-model [^String model]
576 (view
577 (.loadModel
578 (doto (asset-manager)
579 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
580 model)))
582 (defn load-blender-scene [^String model]
583 (.loadModel
584 (doto (asset-manager)
585 (.registerLoader BlenderLoader (into-array String ["blend"])))
586 model))
588 (defn worm
589 []
590 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
592 (defn oto
593 []
594 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
596 (defn sinbad
597 []
598 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
600 (defn worm-blender
601 []
602 (first (seq (.getChildren (load-blender-model
603 "Models/anim2/simple-worm.blend")))))
605 (defn body
606 "given a node with a SkeletonControl, will produce a body sutiable
607 for AI control with movement and proprioception."
608 [node]
609 (let [skeleton-control (.getControl node SkeletonControl)
610 krc (KinematicRagdollControl.)]
611 (comment
612 (dorun
613 (map #(.addBoneName krc %)
614 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
615 ;;"mid2" "mid3" "tail" "head"]
616 )))
617 (.addControl node krc)
618 (.setRagdollMode krc)
619 )
620 node
621 )
622 (defn show-skeleton [node]
623 (let [sd
625 (doto
626 (SkeletonDebugger. "aurellem-skel-debug"
627 (skel node))
628 (.setMaterial (green-x-ray)))]
629 (.attachChild node sd)
630 node))
634 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
636 ;; this could be a good way to give objects special properties like
637 ;; being eyes and the like
639 (.getUserData
640 (.getChild
641 (load-blender-model "Models/property/test.blend") 0)
642 "properties")
644 ;; the properties are saved along with the blender file.
645 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
650 (defn init-debug-skel-node
651 [f debug-node skeleton]
652 (let [bones
653 (map #(.getBone skeleton %)
654 (range (.getBoneCount skeleton)))]
655 (dorun (map #(.setUserControl % true) bones))
656 (dorun (map (fn [b]
657 (println (.getName b)
658 " -- " (f b)))
659 bones))
660 (dorun
661 (map #(.attachChild
662 debug-node
663 (doto
664 (sphere 0.1
665 :position (f %)
666 :physical? false)
667 (.setMaterial (green-x-ray))))
668 bones)))
669 debug-node)
671 (import jme3test.bullet.PhysicsTestHelper)
674 (defn test-zzz [the-worm world value]
675 (if (not value)
676 (let [skeleton (skel the-worm)]
677 (println-repl "enabling bones")
678 (dorun
679 (map
680 #(.setUserControl (.getBone skeleton %) true)
681 (range (.getBoneCount skeleton))))
684 (let [b (.getBone skeleton 2)]
685 (println-repl "moving " (.getName b))
686 (println-repl (.getLocalPosition b))
687 (.setUserTransforms b
688 Vector3f/UNIT_X
689 Quaternion/IDENTITY
690 ;;(doto (Quaternion.)
691 ;; (.fromAngles (/ Math/PI 2)
692 ;; 0
693 ;; 0
695 (Vector3f. 1 1 1))
696 )
698 (println-repl "hi! <3"))))
701 (defn test-ragdoll []
703 (let [the-worm
705 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
706 (doto (show-skeleton (worm-blender))
707 (.setLocalTranslation (Vector3f. 0 10 0))
708 ;;(worm)
709 ;;(oto)
710 ;;(sinbad)
711 )
712 ]
715 (.start
716 (world
717 (doto (Node.)
718 (.attachChild the-worm))
719 {"key-return" (fire-cannon-ball)
720 "key-space" (partial test-zzz the-worm)
721 }
722 (fn [world]
723 (light-up-everything world)
724 (PhysicsTestHelper/createPhysicsTestWorld
725 (.getRootNode world)
726 (asset-manager)
727 (.getPhysicsSpace
728 (.getState (.getStateManager world) BulletAppState)))
729 (set-gravity world Vector3f/ZERO)
730 ;;(.setTimer world (NanoTimer.))
731 ;;(org.lwjgl.input.Mouse/setGrabbed false)
732 )
733 no-op
734 )
737 )))
740 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
741 ;;; here is the ragdoll stuff
743 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
744 (def mesh worm-mesh)
746 (.getFloatBuffer mesh VertexBuffer$Type/Position)
747 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
748 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
751 (defn position [index]
752 (.get
753 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
754 index))
756 (defn bones [index]
757 (.get
758 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
759 index))
761 (defn bone-weights [index]
762 (.get
763 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
764 index))
768 (defn vertex-bones [vertex]
769 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
771 (defn vertex-weights [vertex]
772 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
774 (defn vertex-position [index]
775 (let [offset (* index 3)]
776 (Vector3f. (position offset)
777 (position (inc offset))
778 (position (inc(inc offset))))))
780 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
782 (defn bone-control-color [index]
783 (get {[1 0 0 0] ColorRGBA/Red
784 [1 2 0 0] ColorRGBA/Magenta
785 [2 0 0 0] ColorRGBA/Blue}
786 (vertex-bones index)
787 ColorRGBA/White))
789 (defn influence-color [index bone-num]
790 (get
791 {(float 0) ColorRGBA/Blue
792 (float 0.5) ColorRGBA/Green
793 (float 1) ColorRGBA/Red}
794 ;; find the weight of the desired bone
795 ((zipmap (vertex-bones index)(vertex-weights index))
796 bone-num)
797 ColorRGBA/Blue))
799 (def worm-vertices (set (map vertex-info (range 60))))
802 (defn test-info []
803 (let [points (Node.)]
804 (dorun
805 (map #(.attachChild points %)
806 (map #(sphere 0.01
807 :position (vertex-position %)
808 :color (influence-color % 1)
809 :physical? false)
810 (range 60))))
811 (view points)))
814 (defrecord JointControl [joint physics-space]
815 PhysicsControl
816 (setPhysicsSpace [this space]
817 (dosync
818 (ref-set (:physics-space this) space))
819 (.addJoint space (:joint this)))
820 (update [this tpf])
821 (setSpatial [this spatial])
822 (render [this rm vp])
823 (getPhysicsSpace [this] (deref (:physics-space this)))
824 (isEnabled [this] true)
825 (setEnabled [this state]))
827 (defn add-joint
828 "Add a joint to a particular object. When the object is added to the
829 PhysicsSpace of a simulation, the joint will also be added"
830 [object joint]
831 (let [control (JointControl. joint (ref nil))]
832 (.addControl object control))
833 object)
836 (defn hinge-world
837 []
838 (let [sphere1 (sphere)
839 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
840 joint (Point2PointJoint.
841 (.getControl sphere1 RigidBodyControl)
842 (.getControl sphere2 RigidBodyControl)
843 Vector3f/ZERO (Vector3f. 3 3 3))]
844 (add-joint sphere1 joint)
845 (doto (Node. "hinge-world")
846 (.attachChild sphere1)
847 (.attachChild sphere2))))
850 (defn test-joint []
851 (view (hinge-world)))
853 ;; (defn copier-gen []
854 ;; (let [count (atom 0)]
855 ;; (fn [in]
856 ;; (swap! count inc)
857 ;; (clojure.contrib.duck-streams/copy
858 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
859 ;; ;;/home/r/tmp/mao-test/clojure-images
860 ;; (format "%08d.png" @count)))))))
861 ;; (defn decrease-framerate []
862 ;; (map
863 ;; (copier-gen)
864 ;; (sort
865 ;; (map first
866 ;; (partition
867 ;; 4
868 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
869 ;; (file-seq
870 ;; (file-str
871 ;; "/home/r/media/anime/mao-temp/images"))))))))
875 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
877 (defn proprioception
878 "Create a proprioception map that reports the rotations of the
879 various limbs of the creature's body"
880 [creature]
881 [#^Node creature]
882 (let [
883 nodes (node-seq creature)
884 joints
885 (map
886 :joint
887 (filter
888 #(isa? (class %) JointControl)
889 (reduce
890 concat
891 (map (fn [node]
892 (map (fn [num] (.getControl node num))
893 (range (.getNumControls node))))
894 nodes))))]
895 (fn []
896 (reduce concat (map relative-positions (list (first joints)))))))
899 (defn skel [node]
900 (doto
901 (.getSkeleton
902 (.getControl node SkeletonControl))
903 ;; this is necessary to force the skeleton to have accurate world
904 ;; transforms before it is rendered to the screen.
905 (.resetAndUpdate)))
907 (defn green-x-ray []
908 (doto (Material. (asset-manager)
909 "Common/MatDefs/Misc/Unshaded.j3md")
910 (.setColor "Color" ColorRGBA/Green)
911 (-> (.getAdditionalRenderState)
912 (.setDepthTest false))))
914 (defn test-worm []
915 (.start
916 (world
917 (doto (Node.)
918 ;;(.attachChild (point-worm))
919 (.attachChild (load-blender-model
920 "Models/anim2/joint-worm.blend"))
922 (.attachChild (box 10 1 10
923 :position (Vector3f. 0 -2 0) :mass 0
924 :color (ColorRGBA/Gray))))
925 {
926 "key-space" (fire-cannon-ball)
927 }
928 (fn [world]
929 (enable-debug world)
930 (light-up-everything world)
931 ;;(.setTimer world (NanoTimer.))
932 )
933 no-op)))
937 ;; defunct movement stuff
938 (defn torque-controls [control]
939 (let [torques
940 (concat
941 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
942 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
943 [Vector3f/UNIT_X])]
944 (map (fn [torque-axis]
945 (fn [torque]
946 (.applyTorque
947 control
948 (.mult (.mult (.getPhysicsRotation control)
949 torque-axis)
950 (float
951 (* (.getMass control) torque))))))
952 torques)))
954 (defn motor-map
955 "Take a creature and generate a function that will enable fine
956 grained control over all the creature's limbs."
957 [#^Node creature]
958 (let [controls (keep #(.getControl % RigidBodyControl)
959 (node-seq creature))
960 limb-controls (reduce concat (map torque-controls controls))
961 body-control (partial map #(%1 %2) limb-controls)]
962 body-control))
964 (defn test-motor-map
965 "see how torque works."
966 []
967 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
968 :mass 1 :color ColorRGBA/Green)
969 motor-map (motor-map finger)]
970 (world
971 (nodify [finger
972 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
973 :color ColorRGBA/Gray)])
974 standard-debug-controls
975 (fn [world]
976 (set-gravity world Vector3f/ZERO)
977 (light-up-everything world)
978 (.setTimer world (NanoTimer.)))
979 (fn [_ _]
980 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
981 0]))))))
983 (defn joint-proprioception [#^Node parts #^Node joint]
984 (let [[obj-a obj-b] (joint-targets parts joint)
985 joint-rot (.getWorldRotation joint)
986 pre-inv-a (.inverse (.getWorldRotation obj-a))
987 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
988 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
989 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
991 x Vector3f/UNIT_Y
992 y Vector3f/UNIT_Z
993 z Vector3f/UNIT_X
996 tmp-rot-a (.getWorldRotation obj-a)]
997 (println-repl "x:" (.mult tmp-rot-a x))
998 (println-repl "y:" (.mult tmp-rot-a y))
999 (println-repl "z:" (.mult tmp-rot-a z))
1000 (println-repl "rot-a" (.getWorldRotation obj-a))
1001 (println-repl "rot-b" (.getWorldRotation obj-b))
1002 (println-repl "joint-rot" joint-rot)
1003 ;; this function will report proprioceptive information for the
1004 ;; joint.
1005 (fn []
1006 ;; x is the "twist" axis, y and z are the "bend" axes
1007 (let [rot-a (.getWorldRotation obj-a)
1008 ;;inv-a (.inverse rot-a)
1009 rot-b (.getWorldRotation obj-b)
1010 ;;relative (.mult rot-b inv-a)
1011 basis (doto (Matrix3f.)
1012 (.setColumn 0 (.mult rot-a x))
1013 (.setColumn 1 (.mult rot-a y))
1014 (.setColumn 2 (.mult rot-a z)))
1015 rotation-about-joint
1016 (doto (Quaternion.)
1017 (.fromRotationMatrix
1018 (.mult (.invert basis)
1019 (.toRotationMatrix rot-b))))
1020 [yaw roll pitch]
1021 (seq (.toAngles rotation-about-joint nil))]
1022 ;;return euler angles of the quaternion around the new basis
1023 [yaw roll pitch]))))
1025 #+end_src
1033 * COMMENT generate Source
1034 #+begin_src clojure :tangle ../src/cortex/body.clj
1035 <<proprioception>>
1036 <<motor-control>>
1037 #+end_src
1039 #+begin_src clojure :tangle ../src/cortex/test/body.clj
1040 <<test-body>>
1041 #+end_src