view org/body.org @ 178:6fba17a74a57

refactored touch
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 07:05:54 -0700
parents 026f69582022
children d5c597a7aed4
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 * Making a solid, connected body.
9 #+name: joints
10 #+begin_src clojure
11 (ns cortex.body
12 "Assemble a physical creature using the definitions found in a
13 specially prepared blender file. Creates rigid bodies and joints so
14 that a creature can have a physical presense in the simulation."
15 {:author "Robert McIntyre"}
16 (:use (cortex world util sense))
17 (:use clojure.contrib.def)
18 (:import
19 (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)
20 (com.jme3.bullet.joints
21 SixDofJoint Point2PointJoint HingeJoint ConeJoint)
22 com.jme3.bullet.control.RigidBodyControl
23 com.jme3.collision.CollisionResults
24 com.jme3.bounding.BoundingBox
25 com.jme3.scene.Node
26 com.jme3.scene.Geometry
27 com.jme3.bullet.collision.shapes.HullCollisionShape))
29 (defn joint-targets
30 "Return the two closest two objects to the joint object, ordered
31 from bottom to top according to the joint's rotation."
32 [#^Node parts #^Node joint]
33 (loop [radius (float 0.01)]
34 (let [results (CollisionResults.)]
35 (.collideWith
36 parts
37 (BoundingBox. (.getWorldTranslation joint)
38 radius radius radius)
39 results)
40 (let [targets
41 (distinct
42 (map #(.getGeometry %) results))]
43 (if (>= (count targets) 2)
44 (sort-by
45 #(let [v
46 (jme-to-blender
47 (.mult
48 (.inverse (.getWorldRotation joint))
49 (.subtract (.getWorldTranslation %)
50 (.getWorldTranslation joint))))]
51 (println-repl (.getName %) ":" v)
52 (.dot (Vector3f. 1 1 1)
53 v))
54 (take 2 targets))
55 (recur (float (* radius 2))))))))
57 (defmulti joint-dispatch
58 "Translate blender pseudo-joints into real JME joints."
59 (fn [constraints & _]
60 (:type constraints)))
62 (defmethod joint-dispatch :point
63 [constraints control-a control-b pivot-a pivot-b rotation]
64 (println-repl "creating POINT2POINT joint")
65 ;; bullet's point2point joints are BROKEN, so we must use the
66 ;; generic 6DOF joint instead of an actual Point2Point joint!
68 ;; should be able to do this:
69 (comment
70 (Point2PointJoint.
71 control-a
72 control-b
73 pivot-a
74 pivot-b))
76 ;; but instead we must do this:
77 (println-repl "substuting 6DOF joint for POINT2POINT joint!")
78 (doto
79 (SixDofJoint.
80 control-a
81 control-b
82 pivot-a
83 pivot-b
84 false)
85 (.setLinearLowerLimit Vector3f/ZERO)
86 (.setLinearUpperLimit Vector3f/ZERO)
87 ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
88 ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
90 ))
93 (defmethod joint-dispatch :hinge
94 [constraints control-a control-b pivot-a pivot-b rotation]
95 (println-repl "creating HINGE joint")
96 (let [axis
97 (if-let
98 [axis (:axis constraints)]
99 axis
100 Vector3f/UNIT_X)
101 [limit-1 limit-2] (:limit constraints)
102 hinge-axis
103 (.mult
104 rotation
105 (blender-to-jme axis))]
106 (doto
107 (HingeJoint.
108 control-a
109 control-b
110 pivot-a
111 pivot-b
112 hinge-axis
113 hinge-axis)
114 (.setLimit limit-1 limit-2))))
116 (defmethod joint-dispatch :cone
117 [constraints control-a control-b pivot-a pivot-b rotation]
118 (let [limit-xz (:limit-xz constraints)
119 limit-xy (:limit-xy constraints)
120 twist (:twist constraints)]
122 (println-repl "creating CONE joint")
123 (println-repl rotation)
124 (println-repl
125 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
126 (println-repl
127 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
128 (println-repl
129 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
130 (doto
131 (ConeJoint.
132 control-a
133 control-b
134 pivot-a
135 pivot-b
136 rotation
137 rotation)
138 (.setLimit (float limit-xz)
139 (float limit-xy)
140 (float twist)))))
142 (defn connect
143 "Create a joint between 'obj-a and 'obj-b at the location of
144 'joint. The type of joint is determined by the metadata on 'joint.
146 Here are some examples:
147 {:type :point}
148 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
149 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
151 {:type :cone :limit-xz 0]
152 :limit-xy 0]
153 :twist 0]} (use XZY rotation mode in blender!)"
154 [#^Node obj-a #^Node obj-b #^Node joint]
155 (let [control-a (.getControl obj-a RigidBodyControl)
156 control-b (.getControl obj-b RigidBodyControl)
157 joint-center (.getWorldTranslation joint)
158 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
159 pivot-a (world-to-local obj-a joint-center)
160 pivot-b (world-to-local obj-b joint-center)]
162 (if-let [constraints
163 (map-vals
164 eval
165 (read-string
166 (meta-data joint "joint")))]
167 ;; A side-effect of creating a joint registers
168 ;; it with both physics objects which in turn
169 ;; will register the joint with the physics system
170 ;; when the simulation is started.
171 (do
172 (println-repl "creating joint between"
173 (.getName obj-a) "and" (.getName obj-b))
174 (joint-dispatch constraints
175 control-a control-b
176 pivot-a pivot-b
177 joint-rotation))
178 (println-repl "could not find joint meta-data!"))))
180 (defvar
181 ^{:arglists '([creature])}
182 joints
183 (sense-nodes "joints")
184 "Return the children of the creature's \"joints\" node.")
186 (defn physical!
187 "Iterate through the nodes in creature and make them real physical
188 objects in the simulation."
189 [#^Node creature]
190 (dorun
191 (map
192 (fn [geom]
193 (let [physics-control
194 (RigidBodyControl.
195 (HullCollisionShape.
196 (.getMesh geom))
197 (if-let [mass (meta-data geom "mass")]
198 (do
199 (println-repl
200 "setting" (.getName geom) "mass to" (float mass))
201 (float mass))
202 (float 1)))]
204 (.addControl geom physics-control)))
205 (filter #(isa? (class %) Geometry )
206 (node-seq creature)))))
208 (defn joints!
209 "Connect the solid parts of the creature with physical joints. The
210 joints are taken from the \"joints\" node in the creature."
211 [#^Node creature]
212 (dorun
213 (map
214 (fn [joint]
215 (let [[obj-a obj-b] (joint-targets creature joint)]
216 (connect obj-a obj-b joint)))
217 (joints creature))))
219 (defn body!
220 "Endow the creature with a physical body connected with joints. The
221 particulars of the joints and the masses of each pody part are
222 determined in blender."
223 [#^Node creature]
224 (physical! creature)
225 (joints! creature))
226 #+end_src
228 #+results: joints
229 : #'cortex.body/body!
231 #+results: proprioception
232 : #'cortex.body/proprioception-debug-window
234 * Examples
236 #+name: test-body
237 #+begin_src clojure
238 (ns cortex.test.body
239 (:use (cortex world util body))
240 (:require cortex.silly)
241 (:import
242 com.jme3.math.Vector3f
243 com.jme3.math.ColorRGBA
244 com.jme3.bullet.joints.Point2PointJoint
245 com.jme3.bullet.control.RigidBodyControl
246 com.jme3.system.NanoTimer
247 com.jme3.math.Quaternion))
249 (defn worm-segments
250 "Create multiple evenly spaced box segments. They're fabulous!"
251 [segment-length num-segments interstitial-space radius]
252 (letfn [(nth-segment
253 [n]
254 (box segment-length radius radius :mass 0.1
255 :position
256 (Vector3f.
257 (* 2 n (+ interstitial-space segment-length)) 0 0)
258 :name (str "worm-segment" n)
259 :color (ColorRGBA/randomColor)))]
260 (map nth-segment (range num-segments))))
262 (defn connect-at-midpoint
263 "Connect two physics objects with a Point2Point joint constraint at
264 the point equidistant from both objects' centers."
265 [segmentA segmentB]
266 (let [centerA (.getWorldTranslation segmentA)
267 centerB (.getWorldTranslation segmentB)
268 midpoint (.mult (.add centerA centerB) (float 0.5))
269 pivotA (.subtract midpoint centerA)
270 pivotB (.subtract midpoint centerB)
272 ;; A side-effect of creating a joint registers
273 ;; it with both physics objects which in turn
274 ;; will register the joint with the physics system
275 ;; when the simulation is started.
276 joint (Point2PointJoint.
277 (.getControl segmentA RigidBodyControl)
278 (.getControl segmentB RigidBodyControl)
279 pivotA
280 pivotB)]
281 segmentB))
283 (defn eve-worm
284 "Create a worm-like body bound by invisible joint constraints."
285 []
286 (let [segments (worm-segments 0.2 5 0.1 0.1)]
287 (dorun (map (partial apply connect-at-midpoint)
288 (partition 2 1 segments)))
289 (nodify "worm" segments)))
291 (defn worm-pattern
292 "This is a simple, mindless motor control pattern that drives the
293 second segment of the worm's body at an offset angle with
294 sinusoidally varying strength."
295 [time]
296 (let [angle (* Math/PI (/ 9 20))
297 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
298 [Vector3f/ZERO
299 (.mult
300 direction
301 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
302 Vector3f/ZERO
303 Vector3f/ZERO
304 Vector3f/ZERO]))
306 (defn test-motor-control
307 "Testing motor-control:
308 You should see a multi-segmented worm-like object fall onto the
309 table and begin writhing and moving."
310 []
311 (let [worm (eve-worm)
312 time (atom 0)
313 worm-motor-map (vector-motor-control worm)]
314 (world
315 (nodify [worm
316 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
317 :color ColorRGBA/Gray)])
318 standard-debug-controls
319 (fn [world]
320 (enable-debug world)
321 (light-up-everything world)
322 (comment
323 (com.aurellem.capture.Capture/captureVideo
324 world
325 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
326 )
328 (fn [_ _]
329 (swap! time inc)
330 (Thread/sleep 20)
331 (dorun (worm-motor-map
332 (worm-pattern @time)))))))
336 (defn join-at-point [obj-a obj-b world-pivot]
337 (cortex.silly/joint-dispatch
338 {:type :point}
339 (.getControl obj-a RigidBodyControl)
340 (.getControl obj-b RigidBodyControl)
341 (cortex.silly/world-to-local obj-a world-pivot)
342 (cortex.silly/world-to-local obj-b world-pivot)
343 nil
344 ))
346 (import com.jme3.bullet.collision.PhysicsCollisionObject)
348 (defn blab-* []
349 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
350 :mass 0 :color ColorRGBA/Green)
351 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
352 :mass 1 :color ColorRGBA/Red)
353 connection-point (Vector3f. 1.2 0 0)
354 root (nodify [hand finger])]
356 (join-at-point hand finger (Vector3f. 1.2 0 0))
358 (.setCollisionGroup
359 (.getControl hand RigidBodyControl)
360 PhysicsCollisionObject/COLLISION_GROUP_NONE)
361 (world
362 root
363 standard-debug-controls
364 (fn [world]
365 (enable-debug world)
366 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
367 (set-gravity world Vector3f/ZERO)
368 )
369 no-op)))
370 (comment
372 (defn proprioception-debug-window
373 []
374 (let [time (atom 0)]
375 (fn [prop-data]
376 (if (= 0 (rem (swap! time inc) 40))
377 (println-repl prop-data)))))
378 )
380 (comment
381 (dorun
382 (map
383 (comp
384 println-repl
385 (fn [[p y r]]
386 (format
387 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
388 p y r)))
389 prop-data)))
394 (defn test-proprioception
395 "Testing proprioception:
396 You should see two foating bars, and a printout of pitch, yaw, and
397 roll. Pressing key-r/key-t should move the blue bar up and down and
398 change only the value of pitch. key-f/key-g moves it side to side
399 and changes yaw. key-v/key-b will spin the blue segment clockwise
400 and counterclockwise, and only affect roll."
401 []
402 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
403 :mass 0 :color ColorRGBA/Green :name "hand")
404 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
405 :mass 1 :color ColorRGBA/Red :name "finger")
406 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
407 :position (Vector3f. 0 1.2 0)
408 :rotation (doto (Quaternion.)
409 (.fromAngleAxis
410 (/ Math/PI 2)
411 (Vector3f. 0 0 1)))
412 :physical? false)
413 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
414 creature (nodify [hand finger joint-node])
415 finger-control (.getControl finger RigidBodyControl)
416 hand-control (.getControl hand RigidBodyControl)]
419 (let
420 ;; *******************************************
422 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
423 :mass 0 :color ColorRGBA/Gray)
425 root (nodify [creature floor])
426 prop (joint-proprioception creature joint-node)
427 prop-view (proprioception-debug-window)
429 controls
430 (merge standard-debug-controls
431 {"key-o"
432 (fn [_ _] (.setEnabled finger-control true))
433 "key-p"
434 (fn [_ _] (.setEnabled finger-control false))
435 "key-k"
436 (fn [_ _] (.setEnabled hand-control true))
437 "key-l"
438 (fn [_ _] (.setEnabled hand-control false))
439 "key-i"
440 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
441 "key-period"
442 (fn [world _]
443 (.setEnabled finger-control false)
444 (.setEnabled hand-control false)
445 (.rotate creature (doto (Quaternion.)
446 (.fromAngleAxis
447 (float (/ Math/PI 15))
448 (Vector3f. 0 0 -1))))
450 (.setEnabled finger-control true)
451 (.setEnabled hand-control true)
452 (set-gravity world (Vector3f. 0 0 0))
453 )
456 }
457 )
459 ]
460 (comment
461 (.setCollisionGroup
462 (.getControl hand RigidBodyControl)
463 PhysicsCollisionObject/COLLISION_GROUP_NONE)
464 )
465 (apply
466 world
467 (with-movement
468 hand
469 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
470 [10 10 10 10 1 1]
471 (with-movement
472 finger
473 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
474 [1 1 10 10 10 10]
475 [root
476 controls
477 (fn [world]
478 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
479 (set-gravity world (Vector3f. 0 0 0))
480 (light-up-everything world))
481 (fn [_ _] (prop-view (list (prop))))]))))))
483 #+end_src
485 #+results: test-body
486 : #'cortex.test.body/test-proprioception
489 * COMMENT code-limbo
490 #+begin_src clojure
491 ;;(.loadModel
492 ;; (doto (asset-manager)
493 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
494 ;; "Models/person/person.blend")
497 (defn load-blender-model
498 "Load a .blend file using an asset folder relative path."
499 [^String model]
500 (.loadModel
501 (doto (asset-manager)
502 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
503 model))
506 (defn view-model [^String model]
507 (view
508 (.loadModel
509 (doto (asset-manager)
510 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
511 model)))
513 (defn load-blender-scene [^String model]
514 (.loadModel
515 (doto (asset-manager)
516 (.registerLoader BlenderLoader (into-array String ["blend"])))
517 model))
519 (defn worm
520 []
521 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
523 (defn oto
524 []
525 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
527 (defn sinbad
528 []
529 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
531 (defn worm-blender
532 []
533 (first (seq (.getChildren (load-blender-model
534 "Models/anim2/simple-worm.blend")))))
536 (defn body
537 "given a node with a SkeletonControl, will produce a body sutiable
538 for AI control with movement and proprioception."
539 [node]
540 (let [skeleton-control (.getControl node SkeletonControl)
541 krc (KinematicRagdollControl.)]
542 (comment
543 (dorun
544 (map #(.addBoneName krc %)
545 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
546 ;;"mid2" "mid3" "tail" "head"]
547 )))
548 (.addControl node krc)
549 (.setRagdollMode krc)
550 )
551 node
552 )
553 (defn show-skeleton [node]
554 (let [sd
556 (doto
557 (SkeletonDebugger. "aurellem-skel-debug"
558 (skel node))
559 (.setMaterial (green-x-ray)))]
560 (.attachChild node sd)
561 node))
565 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
567 ;; this could be a good way to give objects special properties like
568 ;; being eyes and the like
570 (.getUserData
571 (.getChild
572 (load-blender-model "Models/property/test.blend") 0)
573 "properties")
575 ;; the properties are saved along with the blender file.
576 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
581 (defn init-debug-skel-node
582 [f debug-node skeleton]
583 (let [bones
584 (map #(.getBone skeleton %)
585 (range (.getBoneCount skeleton)))]
586 (dorun (map #(.setUserControl % true) bones))
587 (dorun (map (fn [b]
588 (println (.getName b)
589 " -- " (f b)))
590 bones))
591 (dorun
592 (map #(.attachChild
593 debug-node
594 (doto
595 (sphere 0.1
596 :position (f %)
597 :physical? false)
598 (.setMaterial (green-x-ray))))
599 bones)))
600 debug-node)
602 (import jme3test.bullet.PhysicsTestHelper)
605 (defn test-zzz [the-worm world value]
606 (if (not value)
607 (let [skeleton (skel the-worm)]
608 (println-repl "enabling bones")
609 (dorun
610 (map
611 #(.setUserControl (.getBone skeleton %) true)
612 (range (.getBoneCount skeleton))))
615 (let [b (.getBone skeleton 2)]
616 (println-repl "moving " (.getName b))
617 (println-repl (.getLocalPosition b))
618 (.setUserTransforms b
619 Vector3f/UNIT_X
620 Quaternion/IDENTITY
621 ;;(doto (Quaternion.)
622 ;; (.fromAngles (/ Math/PI 2)
623 ;; 0
624 ;; 0
626 (Vector3f. 1 1 1))
627 )
629 (println-repl "hi! <3"))))
632 (defn test-ragdoll []
634 (let [the-worm
636 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
637 (doto (show-skeleton (worm-blender))
638 (.setLocalTranslation (Vector3f. 0 10 0))
639 ;;(worm)
640 ;;(oto)
641 ;;(sinbad)
642 )
643 ]
646 (.start
647 (world
648 (doto (Node.)
649 (.attachChild the-worm))
650 {"key-return" (fire-cannon-ball)
651 "key-space" (partial test-zzz the-worm)
652 }
653 (fn [world]
654 (light-up-everything world)
655 (PhysicsTestHelper/createPhysicsTestWorld
656 (.getRootNode world)
657 (asset-manager)
658 (.getPhysicsSpace
659 (.getState (.getStateManager world) BulletAppState)))
660 (set-gravity world Vector3f/ZERO)
661 ;;(.setTimer world (NanoTimer.))
662 ;;(org.lwjgl.input.Mouse/setGrabbed false)
663 )
664 no-op
665 )
668 )))
671 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
672 ;;; here is the ragdoll stuff
674 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
675 (def mesh worm-mesh)
677 (.getFloatBuffer mesh VertexBuffer$Type/Position)
678 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
679 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
682 (defn position [index]
683 (.get
684 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
685 index))
687 (defn bones [index]
688 (.get
689 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
690 index))
692 (defn bone-weights [index]
693 (.get
694 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
695 index))
699 (defn vertex-bones [vertex]
700 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
702 (defn vertex-weights [vertex]
703 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
705 (defn vertex-position [index]
706 (let [offset (* index 3)]
707 (Vector3f. (position offset)
708 (position (inc offset))
709 (position (inc(inc offset))))))
711 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
713 (defn bone-control-color [index]
714 (get {[1 0 0 0] ColorRGBA/Red
715 [1 2 0 0] ColorRGBA/Magenta
716 [2 0 0 0] ColorRGBA/Blue}
717 (vertex-bones index)
718 ColorRGBA/White))
720 (defn influence-color [index bone-num]
721 (get
722 {(float 0) ColorRGBA/Blue
723 (float 0.5) ColorRGBA/Green
724 (float 1) ColorRGBA/Red}
725 ;; find the weight of the desired bone
726 ((zipmap (vertex-bones index)(vertex-weights index))
727 bone-num)
728 ColorRGBA/Blue))
730 (def worm-vertices (set (map vertex-info (range 60))))
733 (defn test-info []
734 (let [points (Node.)]
735 (dorun
736 (map #(.attachChild points %)
737 (map #(sphere 0.01
738 :position (vertex-position %)
739 :color (influence-color % 1)
740 :physical? false)
741 (range 60))))
742 (view points)))
745 (defrecord JointControl [joint physics-space]
746 PhysicsControl
747 (setPhysicsSpace [this space]
748 (dosync
749 (ref-set (:physics-space this) space))
750 (.addJoint space (:joint this)))
751 (update [this tpf])
752 (setSpatial [this spatial])
753 (render [this rm vp])
754 (getPhysicsSpace [this] (deref (:physics-space this)))
755 (isEnabled [this] true)
756 (setEnabled [this state]))
758 (defn add-joint
759 "Add a joint to a particular object. When the object is added to the
760 PhysicsSpace of a simulation, the joint will also be added"
761 [object joint]
762 (let [control (JointControl. joint (ref nil))]
763 (.addControl object control))
764 object)
767 (defn hinge-world
768 []
769 (let [sphere1 (sphere)
770 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
771 joint (Point2PointJoint.
772 (.getControl sphere1 RigidBodyControl)
773 (.getControl sphere2 RigidBodyControl)
774 Vector3f/ZERO (Vector3f. 3 3 3))]
775 (add-joint sphere1 joint)
776 (doto (Node. "hinge-world")
777 (.attachChild sphere1)
778 (.attachChild sphere2))))
781 (defn test-joint []
782 (view (hinge-world)))
784 ;; (defn copier-gen []
785 ;; (let [count (atom 0)]
786 ;; (fn [in]
787 ;; (swap! count inc)
788 ;; (clojure.contrib.duck-streams/copy
789 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
790 ;; ;;/home/r/tmp/mao-test/clojure-images
791 ;; (format "%08d.png" @count)))))))
792 ;; (defn decrease-framerate []
793 ;; (map
794 ;; (copier-gen)
795 ;; (sort
796 ;; (map first
797 ;; (partition
798 ;; 4
799 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
800 ;; (file-seq
801 ;; (file-str
802 ;; "/home/r/media/anime/mao-temp/images"))))))))
806 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
808 (defn proprioception
809 "Create a proprioception map that reports the rotations of the
810 various limbs of the creature's body"
811 [creature]
812 [#^Node creature]
813 (let [
814 nodes (node-seq creature)
815 joints
816 (map
817 :joint
818 (filter
819 #(isa? (class %) JointControl)
820 (reduce
821 concat
822 (map (fn [node]
823 (map (fn [num] (.getControl node num))
824 (range (.getNumControls node))))
825 nodes))))]
826 (fn []
827 (reduce concat (map relative-positions (list (first joints)))))))
830 (defn skel [node]
831 (doto
832 (.getSkeleton
833 (.getControl node SkeletonControl))
834 ;; this is necessary to force the skeleton to have accurate world
835 ;; transforms before it is rendered to the screen.
836 (.resetAndUpdate)))
838 (defn green-x-ray []
839 (doto (Material. (asset-manager)
840 "Common/MatDefs/Misc/Unshaded.j3md")
841 (.setColor "Color" ColorRGBA/Green)
842 (-> (.getAdditionalRenderState)
843 (.setDepthTest false))))
845 (defn test-worm []
846 (.start
847 (world
848 (doto (Node.)
849 ;;(.attachChild (point-worm))
850 (.attachChild (load-blender-model
851 "Models/anim2/joint-worm.blend"))
853 (.attachChild (box 10 1 10
854 :position (Vector3f. 0 -2 0) :mass 0
855 :color (ColorRGBA/Gray))))
856 {
857 "key-space" (fire-cannon-ball)
858 }
859 (fn [world]
860 (enable-debug world)
861 (light-up-everything world)
862 ;;(.setTimer world (NanoTimer.))
863 )
864 no-op)))
868 ;; defunct movement stuff
869 (defn torque-controls [control]
870 (let [torques
871 (concat
872 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
873 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
874 [Vector3f/UNIT_X])]
875 (map (fn [torque-axis]
876 (fn [torque]
877 (.applyTorque
878 control
879 (.mult (.mult (.getPhysicsRotation control)
880 torque-axis)
881 (float
882 (* (.getMass control) torque))))))
883 torques)))
885 (defn motor-map
886 "Take a creature and generate a function that will enable fine
887 grained control over all the creature's limbs."
888 [#^Node creature]
889 (let [controls (keep #(.getControl % RigidBodyControl)
890 (node-seq creature))
891 limb-controls (reduce concat (map torque-controls controls))
892 body-control (partial map #(%1 %2) limb-controls)]
893 body-control))
895 (defn test-motor-map
896 "see how torque works."
897 []
898 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
899 :mass 1 :color ColorRGBA/Green)
900 motor-map (motor-map finger)]
901 (world
902 (nodify [finger
903 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
904 :color ColorRGBA/Gray)])
905 standard-debug-controls
906 (fn [world]
907 (set-gravity world Vector3f/ZERO)
908 (light-up-everything world)
909 (.setTimer world (NanoTimer.)))
910 (fn [_ _]
911 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
912 0]))))))
914 (defn joint-proprioception [#^Node parts #^Node joint]
915 (let [[obj-a obj-b] (joint-targets parts joint)
916 joint-rot (.getWorldRotation joint)
917 pre-inv-a (.inverse (.getWorldRotation obj-a))
918 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
919 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
920 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
922 x Vector3f/UNIT_Y
923 y Vector3f/UNIT_Z
924 z Vector3f/UNIT_X
927 tmp-rot-a (.getWorldRotation obj-a)]
928 (println-repl "x:" (.mult tmp-rot-a x))
929 (println-repl "y:" (.mult tmp-rot-a y))
930 (println-repl "z:" (.mult tmp-rot-a z))
931 (println-repl "rot-a" (.getWorldRotation obj-a))
932 (println-repl "rot-b" (.getWorldRotation obj-b))
933 (println-repl "joint-rot" joint-rot)
934 ;; this function will report proprioceptive information for the
935 ;; joint.
936 (fn []
937 ;; x is the "twist" axis, y and z are the "bend" axes
938 (let [rot-a (.getWorldRotation obj-a)
939 ;;inv-a (.inverse rot-a)
940 rot-b (.getWorldRotation obj-b)
941 ;;relative (.mult rot-b inv-a)
942 basis (doto (Matrix3f.)
943 (.setColumn 0 (.mult rot-a x))
944 (.setColumn 1 (.mult rot-a y))
945 (.setColumn 2 (.mult rot-a z)))
946 rotation-about-joint
947 (doto (Quaternion.)
948 (.fromRotationMatrix
949 (.mult (.invert basis)
950 (.toRotationMatrix rot-b))))
951 [yaw roll pitch]
952 (seq (.toAngles rotation-about-joint nil))]
953 ;;return euler angles of the quaternion around the new basis
954 [yaw roll pitch]))))
956 #+end_src
964 * COMMENT generate Source
965 #+begin_src clojure :tangle ../src/cortex/body.clj
966 <<joints>>
967 #+end_src
969 #+begin_src clojure :tangle ../src/cortex/test/body.clj
970 <<test-body>>
971 #+end_src