rlm@0: #+title: The BODY!!!
rlm@0: #+author: Robert McIntyre
rlm@0: #+email: rlm@mit.edu
rlm@4: #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
rlm@4: #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4: #+INCLUDE: ../../aurellem/org/level-0.org
rlm@4: 
rlm@58: * Body  
rlm@0: 
rlm@0: #+srcname: body-main
rlm@0: #+begin_src clojure 
rlm@44: (ns cortex.body
rlm@44:   (use (cortex world util import)))
rlm@44: 
rlm@0: (use 'clojure.contrib.def)
rlm@0: (cortex.import/mega-import-jme3)
rlm@0: (rlm.rlm-commands/help)
rlm@0: 
rlm@44: ;;(.loadModel 
rlm@44: ;; (doto (asset-manager)
rlm@44: ;;   (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@44: ;;   "Models/person/person.blend")
rlm@44: 
rlm@44: (defn view-model [^String model]
rlm@44:   (view 
rlm@44:    (.loadModel 
rlm@44:     (doto (asset-manager)
rlm@44:       (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@44:     model)))
rlm@49: 
rlm@49: (defn load-blender-scene [^String model]
rlm@49:   (.loadModel
rlm@49:    (doto (asset-manager)
rlm@49:      (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@49:     model))
rlm@49: 
rlm@49: (defn load-blender-model 
rlm@49:   [^String model]
rlm@49:   (.loadModel
rlm@49:    (doto (asset-manager)
rlm@49:      (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@49:    model))
rlm@49: 
rlm@49: (defn worm
rlm@49:   []
rlm@50:   (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@49: 
rlm@50: (defn oto
rlm@49:   []
rlm@49:   (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@49: 
rlm@50: (defn sinbad
rlm@50:   []
rlm@50:   (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@50: 
rlm@50: (defn worm-blender
rlm@50:   []
rlm@50:   (first (seq (.getChildren (load-blender-model
rlm@56:                              "Models/anim2/simple-worm.blend")))))
rlm@50: 
rlm@52: (defn skel [node]
rlm@52:   (doto
rlm@52:       (.getSkeleton
rlm@52:        (.getControl node SkeletonControl))
rlm@52:     ;; this is necessary to force the skeleton to have accurate world
rlm@52:     ;; transforms before it is rendered to the screen. 
rlm@52:     (.resetAndUpdate)))
rlm@52: 
rlm@50: (defprotocol Textual
rlm@50:   (text [something]
rlm@50:     "Display a detailed textual analysis of the given object."))
rlm@50: 
rlm@50: (extend-type com.jme3.scene.Node
rlm@50:   Textual
rlm@50:   (text [node]
rlm@50:     (println "Total Vertexes: " (.getVertexCount node))
rlm@50:     (println "Total Triangles: " (.getTriangleCount node))
rlm@50:     (println "Controls :")
rlm@50:     (dorun (map #(text (.getControl node %)) (range (.getNumControls node))))
rlm@50:     (println "Has " (.getQuantity node) " Children:")
rlm@50:     (doall (map text (.getChildren node)))))
rlm@50: 
rlm@50: (extend-type com.jme3.animation.AnimControl
rlm@50:   Textual
rlm@50:   (text [control]
rlm@50:     (let [animations (.getAnimationNames control)]
rlm@52:       (println "Animation Control with " (count animations) " animation(s):")
rlm@50:       (dorun (map println animations)))))
rlm@50: 
rlm@50: (extend-type com.jme3.animation.SkeletonControl
rlm@50:   Textual
rlm@50:   (text [control]
rlm@50:     (println "Skeleton Control with the following skeleton:")
rlm@50:     (println (.getSkeleton control))))
rlm@50: 
rlm@52: (extend-type com.jme3.bullet.control.KinematicRagdollControl
rlm@52:   Textual
rlm@52:   (text [control]
rlm@52:     (println "Ragdoll Control")))
rlm@52:                  
rlm@52: 
rlm@50: (extend-type com.jme3.scene.Geometry
rlm@50:   Textual
rlm@50:   (text [control]
rlm@50:     (println "...geo...")))
rlm@50: 
rlm@50: 
rlm@50: 
rlm@50: 
rlm@50: (defn body
rlm@52:   "given a node with a SkeletonControl, will produce a body sutiable
rlm@52:   for AI control with movement and proprioception."
rlm@50:   [node]
rlm@50:   (let [skeleton-control (.getControl node SkeletonControl)
rlm@53:         krc (KinematicRagdollControl.)]
rlm@53:     (comment
rlm@50:     (dorun
rlm@50:      (map #(.addBoneName krc %)
rlm@53:           ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@53:            ;;"mid2" "mid3" "tail" "head"]
rlm@53:         )))
rlm@50:     (.addControl node krc)
rlm@52:     (.setRagdollMode krc)
rlm@52:     )
rlm@50:   node
rlm@50:   )
rlm@50: 
rlm@52: (defn green-x-ray []
rlm@51:   (doto (Material. (asset-manager)
rlm@51:                    "Common/MatDefs/Misc/Unshaded.j3md")
rlm@51:     (.setColor "Color" ColorRGBA/Green)
rlm@52:     (-> (.getAdditionalRenderState)
rlm@52:         (.setDepthTest false))))
rlm@51: 
rlm@53: (defn show-skeleton [node]
rlm@50:   (let [sd
rlm@51: 
rlm@50:         (doto
rlm@51:             (SkeletonDebugger. "aurellem-skel-debug"
rlm@51:                                (skel node))
rlm@51:           (.setMaterial (green-x-ray)))]
rlm@53:     (.attachChild node sd)
rlm@53:     node))
rlm@50: 
rlm@51:   
rlm@60: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60: 
rlm@60: ;; this could be a good way to give objects special properties like
rlm@60: ;; being eyes and the like
rlm@60: 
rlm@60: (.getUserData 
rlm@60:  (.getChild
rlm@60:   (load-blender-model "Models/property/test.blend") 0)
rlm@60:  "properties")
rlm@60: 
rlm@60: ;; the properties are saved along with the blender file.
rlm@60: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60: 
rlm@60: 
rlm@50: 
rlm@50: (defn init-debug-skel-node
rlm@50:   [f debug-node skeleton]
rlm@50:   (let [bones 
rlm@50:         (map #(.getBone skeleton %)
rlm@50:              (range (.getBoneCount skeleton)))]
rlm@50:   (dorun (map #(.setUserControl % true) bones)) 
rlm@50:   (dorun (map (fn [b]
rlm@50:                 (println (.getName b)
rlm@50:                          " -- " (f b)))
rlm@50:               bones))
rlm@50:   (dorun
rlm@50:    (map #(.attachChild
rlm@50:           debug-node 
rlm@51:           (doto 
rlm@51:               (sphere 0.1
rlm@51:                       :position (f %)
rlm@51:                       :physical? false)
rlm@51:           (.setMaterial (green-x-ray))))
rlm@50:         bones)))
rlm@50:     debug-node)
rlm@50: 
rlm@52: (import jme3test.bullet.PhysicsTestHelper)
rlm@50: 
rlm@53: 
rlm@53: (defn test-zzz [the-worm world value]
rlm@53:   (if (not value)
rlm@53:     (let [skeleton (skel the-worm)]
rlm@53:       (println-repl "enabling bones")
rlm@53:       (dorun
rlm@53:        (map
rlm@53:         #(.setUserControl (.getBone skeleton %) true)
rlm@53:         (range (.getBoneCount skeleton))))
rlm@53: 
rlm@53: 
rlm@56:       (let [b (.getBone skeleton 2)]
rlm@53:         (println-repl "moving " (.getName b))
rlm@53:         (println-repl (.getLocalPosition b))
rlm@53:         (.setUserTransforms b
rlm@54:                             Vector3f/UNIT_X
rlm@53:                             Quaternion/IDENTITY
rlm@54:                             ;;(doto (Quaternion.)
rlm@54:                              ;; (.fromAngles (/ Math/PI 2)
rlm@54:                              ;;              0
rlm@54:                              ;;              0
rlm@54:                                           
rlm@53:                             (Vector3f. 1 1 1))
rlm@53:         )
rlm@53:       
rlm@53:       (println-repl "hi! <3"))))
rlm@53: 
rlm@53: 
rlm@50: (defn test-ragdoll []
rlm@50:   
rlm@50:   (let [the-worm
rlm@53:         
rlm@52:          ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@53:          (doto (show-skeleton (worm-blender))
rlm@53:            (.setLocalTranslation (Vector3f. 0 10 0))
rlm@53:          ;;(worm)
rlm@52:          ;;(oto)
rlm@52:          ;;(sinbad)
rlm@50:         )
rlm@50:     ]
rlm@50: 
rlm@50: 
rlm@50:     (.start 
rlm@50:      (world
rlm@50:       (doto (Node.)
rlm@50:         (.attachChild the-worm))
rlm@53:       {"key-return" (fire-cannon-ball)
rlm@53:        "key-space" (partial test-zzz the-worm)
rlm@53:        }
rlm@50:       (fn [world]
rlm@50:         (light-up-everything world)
rlm@52:         (PhysicsTestHelper/createPhysicsTestWorld
rlm@52:          (.getRootNode world)
rlm@52:          (asset-manager)
rlm@52:          (.getPhysicsSpace
rlm@52:           (.getState (.getStateManager world) BulletAppState)))
rlm@53:         (set-gravity world Vector3f/ZERO)
rlm@50:         ;;(.setTimer world (NanoTimer.))
rlm@50:         ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@50:         )
rlm@50:       no-op
rlm@50:       )
rlm@50:   
rlm@50:      
rlm@50:      )))
rlm@50:     
rlm@58:        
rlm@58: 
rlm@58: 
rlm@58: 
rlm@58: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@58: ;;; here is the ragdoll stuff
rlm@58: 
rlm@58: (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@58: (def mesh worm-mesh)
rlm@58: 
rlm@58: (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@58: (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@58: (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@58: 
rlm@58: 
rlm@58: (defn position [index]
rlm@58:   (.get
rlm@58:    (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@58:      index))
rlm@58: 
rlm@58: (defn bones [index]
rlm@58:   (.get
rlm@58:    (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@58:    index))
rlm@58: 
rlm@58: (defn bone-weights [index]
rlm@58:   (.get
rlm@58:    (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@58:    index))
rlm@58: 
rlm@58: 
rlm@58: 
rlm@58: (defn vertex-bones [vertex]
rlm@58:   (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@58: 
rlm@58: (defn vertex-weights [vertex]
rlm@58:   (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@58: 
rlm@58: (defn vertex-position [index]
rlm@58:   (let [offset (* index 3)]
rlm@58:     (Vector3f. (position offset)
rlm@58:                (position (inc offset))
rlm@58:                (position (inc(inc offset))))))
rlm@58: 
rlm@58: (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@58: 
rlm@58: (defn bone-control-color [index]
rlm@58:   (get {[1 0 0 0] ColorRGBA/Red
rlm@58:         [1 2 0 0] ColorRGBA/Magenta
rlm@58:         [2 0 0 0] ColorRGBA/Blue}
rlm@58:        (vertex-bones index)
rlm@58:        ColorRGBA/White))
rlm@58: 
rlm@58: (defn influence-color [index bone-num]
rlm@58:   (get
rlm@58:    {(float 0)   ColorRGBA/Blue
rlm@58:     (float 0.5) ColorRGBA/Green
rlm@58:     (float 1)   ColorRGBA/Red}
rlm@58:      ;; find the weight of the desired bone
rlm@58:    ((zipmap (vertex-bones index)(vertex-weights index))
rlm@58:     bone-num)
rlm@58:    ColorRGBA/Blue))
rlm@58:          
rlm@58: (def worm-vertices (set (map vertex-info (range 60))))
rlm@58: 
rlm@58: 
rlm@58: (defn test-info []
rlm@58:   (let [points (Node.)]
rlm@58:     (dorun
rlm@58:      (map #(.attachChild points %)
rlm@58:           (map #(sphere 0.01
rlm@58:                         :position (vertex-position %)
rlm@58:                         :color (influence-color % 1)
rlm@58:                         :physical? false)
rlm@58:                (range 60))))
rlm@58:     (view points)))
rlm@58:   
rlm@58: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@58: 
rlm@60: ;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
rlm@58: 
rlm@58: 
rlm@60: (defrecord JointControl [joint physics-space]
rlm@60:   PhysicsControl
rlm@60:   (setPhysicsSpace [this space]
rlm@60:     (dosync
rlm@60:      (ref-set (:physics-space this) space))
rlm@60:     (.addJoint space (:joint this)))
rlm@60:   (update [this tpf])
rlm@60:   (setSpatial [this spatial])
rlm@60:   (render [this rm vp])
rlm@60:   (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@60:   (isEnabled [this] true)
rlm@60:   (setEnabled [this state]))
rlm@60:   
rlm@55: (defn add-joint
rlm@55:   "Add a joint to a particular object. When the object is added to the
rlm@55:   PhysicsSpace of a simulation, the joint will also be added"
rlm@55:   [object joint]
rlm@60:   (let [control (JointControl. joint (ref nil))]
rlm@55:     (.addControl object control))
rlm@55:   object)
rlm@50: 
rlm@55: (defn hinge-world
rlm@55:   []
rlm@55:   (let [sphere1 (sphere)
rlm@55:         sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@55:         joint (Point2PointJoint.
rlm@55:                (.getControl sphere1 RigidBodyControl)
rlm@55:                (.getControl sphere2 RigidBodyControl) 
rlm@55:                Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@55:     (add-joint sphere1 joint)
rlm@55:     (doto (Node. "hinge-world")
rlm@55:       (.attachChild sphere1)
rlm@55:       (.attachChild sphere2))))
rlm@55: 
rlm@56: (defn test-joint []
rlm@56:   (view (hinge-world)))
rlm@49: 
rlm@49: 
rlm@58: (defn worm [segment-length num-segments interstitial-space radius]
rlm@58:   (letfn [(nth-segment
rlm@58:             [n]
rlm@58:             (box segment-length radius radius :mass 0.1
rlm@58:                  :position
rlm@58:                  (Vector3f.
rlm@58:                   (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@58:                  :name (str "worm-segment" n)
rlm@58:                  :color (ColorRGBA/randomColor)))]
rlm@58:     (map nth-segment (range num-segments))))
rlm@58: 
rlm@58: (defn nodify
rlm@58:   "take a sequence of things that can be attached to a node and return
rlm@60:   a node with all of them attached"
rlm@58:   ([name children]
rlm@58:      (let [node (Node. name)]
rlm@58:        (dorun (map #(.attachChild node %) children))
rlm@58:        node))
rlm@58:   ([children] (nodify "" children)))
rlm@58: 
rlm@58: 
rlm@58: (defn connect-at-midpoint
rlm@58:   [segmentA segmentB]
rlm@58:   (let [centerA (.getWorldTranslation segmentA)
rlm@58:         centerB (.getWorldTranslation segmentB)
rlm@58:         midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@58:         pivotA (.subtract midpoint centerA)
rlm@58:         pivotB (.subtract midpoint centerB)
rlm@58: 
rlm@58:         joint (Point2PointJoint.
rlm@58:                (.getControl segmentA RigidBodyControl)
rlm@58:                (.getControl segmentB RigidBodyControl)
rlm@58:                pivotA
rlm@58:                pivotB)]
rlm@58:     (add-joint segmentA joint)
rlm@58:   segmentB))
rlm@58: 
rlm@58: 
rlm@58: (defn point-worm []
rlm@58:   (let [segments (worm 0.2 5 0.1 0.1)]
rlm@58:     (dorun (map (partial apply connect-at-midpoint)
rlm@58:                 (partition 2 1 segments)))
rlm@58:     (nodify "worm"  segments)))
rlm@58: 
rlm@58: 
rlm@58: (defn test-worm []
rlm@58:   (.start
rlm@58:    (world
rlm@58:     (doto (Node.)
rlm@58:       ;;(.attachChild (point-worm))
rlm@58:       (.attachChild (load-blender-model
rlm@58:                      "Models/anim2/joint-worm.blend"))
rlm@58:                       
rlm@58:       (.attachChild (box 10 1 10
rlm@58:                          :position (Vector3f. 0 -2 0) :mass 0
rlm@58:                          :color (ColorRGBA/Gray))))
rlm@58:     {
rlm@58:      "key-space" (fire-cannon-ball)
rlm@58:      }
rlm@58:     (fn [world]
rlm@58:       (enable-debug world)
rlm@58:       (light-up-everything world)
rlm@58:       ;;(.setTimer world (NanoTimer.))
rlm@58:       )
rlm@58:     no-op)))
rlm@49: 
rlm@56: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@56: 
rlm@56: 
rlm@60: ;;;;;;;;;  Mortor Control  ;;;;;;;;;;;;;
rlm@56: 
rlm@56: 
rlm@60: ;; surprisingly ehough, terristerial creatures only move by using
rlm@60: ;; torque applied to their joints.  There's not a single straight line
rlm@60: ;; of force in the human body at all!  (a straight line of force would
rlm@60: ;; correspond to some sort of jet or rocket propulseion)
rlm@56: 
rlm@60: (defn node-seq
rlm@60:   "take a node and return a seq of all its children
rlm@60:    recursively. There will be no nodes left in the resulting
rlm@60:    structure"
rlm@60:   [#^Node node]
rlm@60:   (tree-seq #(isa? (class %) Node) #(.getChildren %) node))
rlm@60:   
rlm@60:   
rlm@60: (defn torque-controls [control]
rlm@60:   (let [torques
rlm@60:         (concat
rlm@60:          (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@60:               (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@60:          [Vector3f/UNIT_X])]
rlm@60:     (map (fn [torque-axis]
rlm@60:            (fn [torque]
rlm@60:              (.applyTorque
rlm@60:               control
rlm@60:               (.mult (.mult (.getPhysicsRotation control)
rlm@60:                             torque-axis)
rlm@60:                      (float
rlm@60:                       (* (.getMass control) torque))))))
rlm@60:          torques)))
rlm@56: 
rlm@60: (defn motor-map
rlm@60:   "Take a creature and generate a function that will enable fine
rlm@60:    grained control over all the creature's limbs."  
rlm@60:   [#^Node creature]
rlm@60:   (let [controls (keep #(.getControl % RigidBodyControl)
rlm@60:                        (node-seq creature))
rlm@60:         limb-controls (reduce concat (map torque-controls controls))
rlm@60:         body-control  (partial map #(%1 %2) limb-controls)]
rlm@60:     body-control))
rlm@56: 
rlm@56: 
rlm@56: 
rlm@60: (defn test-motor-map
rlm@60:   "see how torque works."
rlm@60:   []
rlm@60:   (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@60:                     :mass 1 :color ColorRGBA/Green)
rlm@60:         motor-map (motor-map finger)]
rlm@60:     (world
rlm@60:      (nodify [finger
rlm@60:               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60:                    :color ColorRGBA/Gray)])
rlm@60:      standard-debug-controls
rlm@60:      (fn [world]
rlm@60:        (set-gravity world Vector3f/ZERO)
rlm@60:        (light-up-everything world)
rlm@60:        (.setTimer world (NanoTimer.)))
rlm@60:      (fn [_ _]
rlm@60:        (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: (defn test-torque
rlm@60:   "see how torque works."
rlm@60:   []
rlm@60:   (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@60:                     :mass 1 :color ColorRGBA/Green)
rlm@60:         move-left? (atom false)
rlm@60:         move-right? (atom false)
rlm@60:         control (.getControl finger RigidBodyControl)]
rlm@60:     (world
rlm@60:      (nodify [finger
rlm@60:               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60:                    :color ColorRGBA/Gray)])
rlm@60:      (merge standard-debug-controls
rlm@60:             {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@60:              "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
rlm@60:      (fn [world]
rlm@60:        (set-gravity world Vector3f/ZERO)
rlm@60:        (light-up-everything world)
rlm@60:        (.setTimer world (NanoTimer.)))
rlm@60:      (fn [_ _]
rlm@60:        (if @move-left?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                        (Vector3f. -3 20 0))))
rlm@60:        (if @move-right?
rlm@60:          (.applyTorque control (Vector3f. 0 0 1)))))))
rlm@60: 
rlm@60: 
rlm@60: (defn worm-pattern [time]
rlm@60:   [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60: 
rlm@60:    0 0 0 0 0 0 0 0 0 0 0
rlm@60: 
rlm@60:    (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
rlm@60: 
rlm@60:    0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60:   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60:   0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60: 
rlm@60:    ])
rlm@60: 
rlm@60: 
rlm@60: ;; (defn copier-gen []
rlm@60: ;;   (let [count (atom 0)]
rlm@60: ;;     (fn [in]
rlm@60: ;;       (swap! count inc)
rlm@60: ;;       (clojure.contrib.duck-streams/copy
rlm@60: ;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@60: ;;                      ;;/home/r/tmp/mao-test/clojure-images
rlm@60: ;;                (format "%08d.png" @count)))))))
rlm@60: ;; (defn decrease-framerate []
rlm@60: ;;   (map
rlm@60: ;;    (copier-gen)
rlm@60: ;;    (sort
rlm@60: ;;     (map first
rlm@60: ;;          (partition
rlm@60: ;;           4
rlm@60: ;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@60: ;;                   (file-seq
rlm@60: ;;                    (file-str
rlm@60: ;;                     "/home/r/media/anime/mao-temp/images"))))))))
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: ;;;;;;;;;;;;;;;;;;  Proprioception  ;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60: 
rlm@60: 
rlm@60: ;; this is not used as just getting the rotation would be simpler.
rlm@60: (defn proprioception-senses 
rlm@60:   "given a control , create a sequence of thunks that will report the
rlm@60:   rotation of the control's object along the same axes as the motor-control map."
rlm@60:   [control]
rlm@60:   (let [torques
rlm@60:         (concat
rlm@60:          (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@60:               (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@60:          [Vector3f/UNIT_X])]
rlm@60:     (map (fn [torque-axis]
rlm@60:            (fn []
rlm@60:              (.getPhysicsRotation control)))
rlm@60:            torques)))
rlm@60: 
rlm@60: (defn orthogonal-vect
rlm@60:   "Return a vector orthogonal to the current one"
rlm@60:   [vector]
rlm@60:   (let [x (.getX vector)
rlm@60:         y (.getY vector)
rlm@60:         z (.getZ vector)]
rlm@60:     (cond
rlm@60:      (not= x (float 0)) (Vector3f. (- z) 0 x)
rlm@60:      (not= y (float 0)) (Vector3f. 0 (- z) y)
rlm@60:      (not= z (float 0)) (Vector3f. 0 (- z) y)
rlm@60:      true Vector3f/ZERO)))
rlm@60: 
rlm@60: ;; from
rlm@60: ;; http://stackoverflow.com/questions/3684269/ \\
rlm@60: ;;   component-of-a-quaternion-rotation-around-an-axis
rlm@60: (defn rot-about-axis [#^Quaternion q #^Vector3f axis]
rlm@60:   (let [basis-1 (orthogonal-vect axis)
rlm@60:         basis-2 (.cross axis basis-1)
rlm@60:         rotated (.mult q basis-1)
rlm@60:         alpha (.dot basis-1 (.project rotated basis-1))
rlm@60:         beta  (.dot basis-2 (.project rotated basis-2))]
rlm@60:     (println-repl alpha)
rlm@60:     (println-repl beta)
rlm@60:     (Math/atan2 beta alpha)))
rlm@60: 
rlm@60: 
rlm@60: (defn check-rot [a]
rlm@60:   (rot-about-axis
rlm@60:    (doto (Quaternion.)
rlm@60:      (.fromAngleAxis
rlm@60:       (float a)
rlm@60:       (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
rlm@60: 
rlm@60: (defn relative-positions [joint]
rlm@60:   (let [object-a (.getUserObject (.getBodyA joint))
rlm@60:         object-b (.getUserObject (.getBodyB joint))
rlm@60:         arm-a
rlm@60:         (.normalize
rlm@60:          (.subtract 
rlm@60:           (.localToWorld object-a (.getPivotA joint) nil)
rlm@60:           (.getWorldTranslation object-a)))
rlm@60:         rotate-a
rlm@60:          (doto (Matrix3f.)
rlm@60:            (.fromStartEndVectors arm-a Vector3f/UNIT_X))
rlm@60:         arm-b
rlm@60:         (.mult
rlm@60:          rotate-a 
rlm@60:          (.normalize
rlm@60:           (.subtract 
rlm@60:            (.localToWorld object-b (.getPivotB joint) nil)
rlm@60:            (.getWorldTranslation object-b))))
rlm@60:         rotate-b
rlm@60:         (doto (Matrix3f.)
rlm@60:           (.fromStartEndVectors arm-b Vector3f/UNIT_X))
rlm@60:         
rlm@60:         pitch
rlm@60:         (.angleBetween
rlm@60:          (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
rlm@60:          (Vector2f. 1 0))
rlm@60:         yaw
rlm@60:         (.angleBetween
rlm@60:          (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
rlm@60:          (Vector2f. 1 0))
rlm@60:                      
rlm@60:         roll
rlm@60:         (.mult 
rlm@60:          (.getLocalRotation object-b)
rlm@60:          (doto (Quaternion.)
rlm@60:            (.fromRotationMatrix rotate-a)))
rlm@60:          ]
rlm@60:          
rlm@60:                 
rlm@60: 
rlm@60:     ;;(println-repl
rlm@60:     ;; "arm-b is " arm-b)
rlm@60:     ;;(println-repl
rlm@60:     ;; "pivot-b is " (.getPivotB joint))
rlm@60:     ;;(println-repl
rlm@60:     ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@60:     ;;         pitch yaw roll))
rlm@60:     [pitch yaw roll]))
rlm@60: 
rlm@60:         
rlm@60:     
rlm@60:         
rlm@60:   
rlm@60: (defn proprioception
rlm@60:   "Create a proprioception map that reports the rotations of the
rlm@60:   various limbs of the creature's body"
rlm@60:   [creature]
rlm@60:   [#^Node creature]
rlm@60:   (let [
rlm@60:         nodes (node-seq creature)
rlm@60:         joints
rlm@60:         (map
rlm@60:          :joint 
rlm@60:          (filter
rlm@60:           #(isa? (class %) JointControl)
rlm@60:           (reduce
rlm@60:            concat
rlm@60:            (map (fn [node]
rlm@60:                   (map (fn [num] (.getControl node num))
rlm@60:                        (range (.getNumControls node))))
rlm@60:                 nodes))))]
rlm@60:     (fn []
rlm@60:       (reduce concat (map relative-positions (list (first joints)))))))
rlm@60:         
rlm@60: 
rlm@60: (defn test-worm-control
rlm@60:   []
rlm@60:   (let [worm (point-worm)
rlm@60:         time (atom 0)
rlm@60:         worm-motor-map (motor-map worm)
rlm@60:         body-map (proprioception worm)
rlm@60:         debug-segments
rlm@60:         (map
rlm@60:          #(doto
rlm@60:               (make-shape
rlm@60:                (assoc base-shape
rlm@60:                  :name "debug-line"
rlm@60:                  :physical? false
rlm@60:                  :shape
rlm@60:                  (com.jme3.scene.shape.Line.
rlm@60:                   (.add (.getWorldTranslation %)
rlm@60:                         (Vector3f. -0.2 0 0 ))
rlm@60:                   (.add (.getWorldTranslation %)
rlm@60:                         (Vector3f. 0.2 0 0)))))
rlm@60:                  (.setMaterial (green-x-ray)))
rlm@60:             (drop 1 (node-seq worm)))]
rlm@60:     (world
rlm@60:      (nodify [worm
rlm@60:               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60:                    :color ColorRGBA/Gray)])
rlm@60:      standard-debug-controls
rlm@60:      (fn [world]
rlm@60:        (.attachChild (.getRootNode world) (nodify debug-segments))
rlm@60:        (enable-debug world)
rlm@60:        (light-up-everything world)
rlm@60:        (com.aurellem.capture.Capture/captureVideo
rlm@60:         world
rlm@60:         (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@60:      
rlm@60:      (fn [_ _]
rlm@60:        (dorun
rlm@60:         (map
rlm@60:          (fn [worm-segment
rlm@60:               debug-segment]
rlm@60:            (.rotate
rlm@60:             debug-segment
rlm@60:             (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
rlm@60:          (drop 1 (node-seq worm))
rlm@60:          debug-segments))
rlm@60:        (swap! time inc)
rlm@60:        (println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))
rlm@60:        (Thread/sleep 200)
rlm@60:        (dorun (worm-motor-map
rlm@60:                (worm-pattern @time)))))))
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: (defn test-prop
rlm@60:   "see how torque works."
rlm@60:   []
rlm@60:   (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
rlm@60:                      :mass 0 :color ColorRGBA/Green)
rlm@60:         finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
rlm@60:                      :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
rlm@60:         floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
rlm@60:                      :mass 0 :color ColorRGBA/Gray)
rlm@60: 
rlm@60:         move-up? (atom false)
rlm@60:         move-down? (atom false)
rlm@60:         move-left? (atom false)
rlm@60:         move-right? (atom false)
rlm@60:         roll-left? (atom false)
rlm@60:         roll-right? (atom false)
rlm@60:         control (.getControl finger RigidBodyControl)
rlm@60:         joint
rlm@60:         (doto 
rlm@60:             (Point2PointJoint.
rlm@60:              (.getControl hand RigidBodyControl)
rlm@60:              control
rlm@60:              (Vector3f. 1.2 0 0)
rlm@60:              (Vector3f. -1.2 0 0 ))
rlm@60:           (.setCollisionBetweenLinkedBodys false))
rlm@60:         time (atom 0)
rlm@60:         ]
rlm@60:     (world
rlm@60:      (nodify [hand finger floor])
rlm@60:      (merge standard-debug-controls
rlm@60:             {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
rlm@60:              "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
rlm@60:              "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@60:              "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
rlm@60:              "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
rlm@60:              "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
rlm@60:      (fn [world]
rlm@60:        (set-gravity world Vector3f/ZERO)
rlm@60:        (.setMoveSpeed (.getFlyByCamera world) 50)
rlm@60:        (.setRotationSpeed (.getFlyByCamera world) 50)
rlm@60:        (light-up-everything world)
rlm@60:        (.setTimer world (NanoTimer.))
rlm@60:        )
rlm@60:      (fn [_ _]
rlm@60:        (if @move-up?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                        (Vector3f. 0 0 1))))
rlm@60:        (if @move-down?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                               (Vector3f. 0 0 -1))))
rlm@60:        (if @move-left?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                               (Vector3f. 0 1 0))))
rlm@60:        (if @move-right?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                               (Vector3f. 0 -1 0))))
rlm@60:        (if @roll-left?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                               (Vector3f. -0.1 0 0))))
rlm@60:        (if @roll-right?
rlm@60:          (.applyTorque control
rlm@60:                        (.mult (.getPhysicsRotation control)
rlm@60:                               (Vector3f. 0.1 0 0))))
rlm@60: 
rlm@60:        (if (= 0 (rem (swap! time inc) 2000))
rlm@60:          (do 
rlm@60:            ;;(println-repl (.getLocalRotation finger))
rlm@60:            (println-repl (nth (relative-positions joint) 2))))
rlm@60: 
rlm@60:        ))))
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: 
rlm@60: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@56: 
rlm@56: 
rlm@56: 
rlm@56: 
rlm@0: #+end_src
rlm@0: 
rlm@60: #+results: body-main
rlm@60: : #'cortex.body/test-prop
rlm@60: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: * COMMENT generate Source.
rlm@44: #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@0: <<body-main>>
rlm@0: #+end_src
rlm@0:   
rlm@60: