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@64: * Proprioception
rlm@66: #+name: proprioception
rlm@0: #+begin_src clojure 
rlm@44: (ns cortex.body
rlm@64:   (:use (cortex world util))
rlm@64:   (:import
rlm@64:    com.jme3.math.Vector3f
rlm@64:    com.jme3.math.Quaternion
rlm@64:    com.jme3.math.Vector2f
rlm@64:    com.jme3.math.Matrix3f
rlm@135:    com.jme3.bullet.control.RigidBodyControl
rlm@135:    com.jme3.collision.CollisionResults
rlm@135:    com.jme3.bounding.BoundingBox))
rlm@44: 
rlm@133: (import com.jme3.scene.Node)
rlm@133: 
rlm@135: (defn jme-to-blender
rlm@135:   "Convert from JME coordinates to Blender coordinates"
rlm@135:   [#^Vector3f in]
rlm@135:   (Vector3f. (.getX in)
rlm@135:              (- (.getZ in))
rlm@135:              (.getY in)))
rlm@135: 
rlm@135: (defn joint-targets
rlm@135:   "Return the two closest two objects to the joint object, ordered
rlm@135:   from bottom to top according to the joint's rotation."
rlm@135:   [#^Node parts #^Node joint]
rlm@135:   (loop [radius (float 0.01)]
rlm@135:     (let [results (CollisionResults.)]
rlm@135:       (.collideWith
rlm@135:        parts
rlm@135:        (BoundingBox. (.getWorldTranslation joint)
rlm@135:                      radius radius radius)
rlm@135:        results)
rlm@135:       (let [targets
rlm@135:             (distinct
rlm@135:              (map  #(.getGeometry %) results))]
rlm@135:         (if (>= (count targets) 2)
rlm@135:           (sort-by
rlm@135:            #(let [v
rlm@135:                   (jme-to-blender
rlm@135:                    (.mult
rlm@135:                     (.inverse (.getWorldRotation joint))
rlm@135:                     (.subtract (.getWorldTranslation %)
rlm@135:                                (.getWorldTranslation joint))))]
rlm@135:               (println-repl (.getName %) ":" v)
rlm@135:               (.dot (Vector3f. 1 1 1)
rlm@135:                     v))                  
rlm@135:            (take 2 targets))
rlm@135:           (recur (float (* radius 2))))))))
rlm@135: 
rlm@135: (defn creature-joints 
rlm@135:   "Return the children of the creature's \"joints\" node."
rlm@135:   [#^Node creature]
rlm@135:   (if-let [joint-node (.getChild creature "joints")]
rlm@135:     (seq (.getChildren joint-node))
rlm@135:     (do (println-repl "could not find JOINTS node") [])))
rlm@135: 
rlm@133: (defn joint-proprioception [#^Node parts #^Node joint]
rlm@135:   (let [[obj-a obj-b] (joint-targets parts joint)
rlm@133:         joint-rot (.getWorldRotation joint)
rlm@137:         pre-inv-a (.inverse (.getWorldRotation obj-a))
rlm@137:         x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
rlm@137:         y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
rlm@137:         z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))]
rlm@137:     (println-repl "x:" x)
rlm@137:     (println-repl "y:" y)
rlm@137:     (println-repl "z:" z)
rlm@133:     ;; this function will report proprioceptive information for the
rlm@135:     ;; joint.
rlm@133:     (fn []
rlm@133:       ;; x is the "twist" axis, y and z are the "bend" axes
rlm@133:       (let [rot-a (.getWorldRotation obj-a)
rlm@136:             ;;inv-a (.inverse rot-a)
rlm@133:             rot-b (.getWorldRotation obj-b)
rlm@136:             ;;relative (.mult rot-b inv-a)
rlm@133:             basis (doto (Matrix3f.)
rlm@136:                     (.setColumn 0 (.mult rot-a x))
rlm@136:                     (.setColumn 1 (.mult rot-a y))
rlm@136:                     (.setColumn 2 (.mult rot-a z)))
rlm@133:             rotation-about-joint
rlm@133:             (doto (Quaternion.)
rlm@133:               (.fromRotationMatrix 
rlm@133:                (.mult (.invert basis)
rlm@136:                       (.toRotationMatrix rot-b))))
rlm@133:             [yaw roll pitch]
rlm@133:             (seq (.toAngles rotation-about-joint nil))]
rlm@133:         ;;return euler angles of the quaternion around the new basis            
rlm@133:         [yaw roll pitch]
rlm@133:         ))))
rlm@133: 
rlm@133: 
rlm@63: (defn proprioception
rlm@63:   "Create a function that provides proprioceptive information about an
rlm@63:   entire body."
rlm@134:   [#^Node creature]
rlm@63:   ;; extract the body's joints
rlm@135:   (let [joints (creature-joints creature)
rlm@134:         senses (map (partial joint-proprioception creature) joints)]
rlm@63:     (fn []
rlm@134:       (map #(%) senses))))
rlm@141: 
rlm@141: (defn tap [obj direction force]
rlm@141:   (let [control (.getControl obj RigidBodyControl)]
rlm@141:     (.applyTorque
rlm@141:      control
rlm@141:      (.mult (.getPhysicsRotation control)
rlm@141:             (.mult (.normalize direction) (float force))))))
rlm@141: 
rlm@141: 
rlm@141: (defn with-movement
rlm@141:   [object
rlm@141:    [up down left right roll-up roll-down :as keyboard]
rlm@141:    forces
rlm@141:    [root-node
rlm@141:     keymap
rlm@141:     intilization
rlm@141:     world-loop]]
rlm@141:   (let [add-keypress
rlm@141:         (fn [state keymap key]
rlm@141:           (merge keymap
rlm@141:                   {key
rlm@141:                    (fn [_ pressed?]
rlm@141:                      (reset! state pressed?))}))
rlm@141:         move-up? (atom false)
rlm@141:         move-down? (atom false)
rlm@141:         move-left? (atom false)
rlm@141:         move-right? (atom false)
rlm@141:         roll-left? (atom false)
rlm@141:         roll-right? (atom false)
rlm@141:         
rlm@141:         directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0)
rlm@141:                     (Vector3f. 0 0 1)(Vector3f. 0 0 -1)
rlm@141:                     (Vector3f. -1 0 0)(Vector3f. 1 0 0)]
rlm@141:         atoms [move-left? move-right? move-up? move-down? 
rlm@141:                  roll-left? roll-right?]
rlm@141: 
rlm@141:         keymap* (reduce merge
rlm@141:                         (map #(add-keypress %1 keymap %2)
rlm@141:                              atoms
rlm@141:                              keyboard))
rlm@141:         
rlm@141:         splice-loop (fn []
rlm@141:                       (dorun
rlm@141:                        (map
rlm@141:                         (fn [sym direction force]
rlm@141:                           (if @sym
rlm@141:                             (tap object direction force)))
rlm@141:                         atoms directions forces)))
rlm@141: 
rlm@141:         world-loop* (fn [world tpf]
rlm@141:                        (world-loop world tpf)
rlm@141:                        (splice-loop))]
rlm@141: 
rlm@141:      [root-node
rlm@141:       keymap*
rlm@141:       intilization
rlm@141:       world-loop*]))
rlm@141: 
rlm@60:   
rlm@64: #+end_src
rlm@63: 
rlm@133: #+results: proprioception
rlm@133: : #'cortex.body/proprioception
rlm@133: 
rlm@65: * Motor Control
rlm@66: #+name: motor-control
rlm@64: #+begin_src clojure
rlm@64: (in-ns 'cortex.body)
rlm@63: 
rlm@63: ;; surprisingly enough, terristerial creatures only move by using
rlm@63: ;; torque applied about their joints.  There's not a single straight
rlm@63: ;; line of force in the human body at all!  (A straight line of force
rlm@63: ;; would correspond to some sort of jet or rocket propulseion.)
rlm@63: 
rlm@63: (defn vector-motor-control
rlm@63:   "Create a function that accepts a sequence of Vector3f objects that
rlm@63:    describe the torque to be applied to each part of the body."
rlm@63:   [body]
rlm@63:   (let [nodes (node-seq body)
rlm@63:         controls (keep #(.getControl % RigidBodyControl) nodes)]
rlm@63:     (fn [torques]
rlm@63:       (map #(.applyTorque %1 %2)
rlm@63:            controls torques))))
rlm@64: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@64: #+end_src
rlm@64:  
rlm@64: ## note -- might want to add a lower dimensional, discrete version of
rlm@64: ## this if it proves useful from a x-modal clustering perspective.
rlm@63: 
rlm@64: * Examples
rlm@63: 
rlm@69: #+name: test-body
rlm@64: #+begin_src clojure
rlm@69: (ns cortex.test.body
rlm@64:   (:use (cortex world util body))
rlm@135:   (:require cortex.silly)
rlm@64:   (:import
rlm@64:    com.jme3.math.Vector3f
rlm@64:    com.jme3.math.ColorRGBA
rlm@64:    com.jme3.bullet.joints.Point2PointJoint
rlm@64:    com.jme3.bullet.control.RigidBodyControl
rlm@64:    com.jme3.system.NanoTimer))
rlm@63: 
rlm@64: (defn worm-segments
rlm@64:   "Create multiple evenly spaced box segments. They're fabulous!"
rlm@64:   [segment-length num-segments interstitial-space radius]
rlm@64:   (letfn [(nth-segment
rlm@64:             [n]
rlm@64:             (box segment-length radius radius :mass 0.1
rlm@64:                  :position
rlm@64:                  (Vector3f.
rlm@64:                   (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@64:                  :name (str "worm-segment" n)
rlm@64:                  :color (ColorRGBA/randomColor)))]
rlm@64:     (map nth-segment (range num-segments))))
rlm@63: 
rlm@64: (defn connect-at-midpoint
rlm@64:   "Connect two physics objects with a Point2Point joint constraint at
rlm@64:   the point equidistant from both objects' centers."
rlm@64:   [segmentA segmentB]
rlm@64:   (let [centerA (.getWorldTranslation segmentA)
rlm@64:         centerB (.getWorldTranslation segmentB)
rlm@64:         midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@64:         pivotA (.subtract midpoint centerA)
rlm@64:         pivotB (.subtract midpoint centerB)
rlm@64:         
rlm@64:         ;; A side-effect of creating a joint registers
rlm@64:         ;; it with both physics objects which in turn
rlm@64:         ;; will register the joint with the physics system
rlm@64:         ;; when the simulation is started.
rlm@64:         joint (Point2PointJoint.
rlm@64:                (.getControl segmentA RigidBodyControl)
rlm@64:                (.getControl segmentB RigidBodyControl)
rlm@64:                pivotA
rlm@64:                pivotB)]
rlm@64:     segmentB))
rlm@63: 
rlm@64: (defn eve-worm
rlm@72:   "Create a worm-like body bound by invisible joint constraints."
rlm@64:   []
rlm@64:   (let [segments (worm-segments 0.2 5 0.1 0.1)]
rlm@64:     (dorun (map (partial apply connect-at-midpoint)
rlm@64:                 (partition 2 1 segments)))
rlm@64:     (nodify "worm" segments)))
rlm@63: 
rlm@64: (defn worm-pattern
rlm@64:   "This is a simple, mindless motor control pattern that drives the
rlm@64:    second segment of the worm's body at an offset angle with
rlm@64:    sinusoidally varying strength."
rlm@64:   [time]
rlm@64:   (let [angle (* Math/PI (/ 9 20))
rlm@63:         direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
rlm@63:     [Vector3f/ZERO
rlm@63:      (.mult
rlm@63:       direction
rlm@63:       (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
rlm@63:      Vector3f/ZERO
rlm@63:      Vector3f/ZERO
rlm@63:      Vector3f/ZERO]))
rlm@60: 
rlm@64: (defn test-motor-control
rlm@69:   "Testing motor-control:
rlm@69:    You should see a multi-segmented worm-like object fall onto the
rlm@64:    table and begin writhing and moving."
rlm@60:   []
rlm@64:   (let [worm (eve-worm)
rlm@60:         time (atom 0)
rlm@63:         worm-motor-map (vector-motor-control 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:        (enable-debug world)
rlm@60:        (light-up-everything world)
rlm@63:        (comment
rlm@63:          (com.aurellem.capture.Capture/captureVideo
rlm@63:           world
rlm@63:           (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@63:        )
rlm@60:      
rlm@60:      (fn [_ _]
rlm@60:        (swap! time inc)
rlm@64:        (Thread/sleep 20)
rlm@60:        (dorun (worm-motor-map
rlm@60:                (worm-pattern @time)))))))
rlm@60: 
rlm@130: 
rlm@135: 
rlm@130: (defn join-at-point [obj-a obj-b world-pivot]
rlm@130:   (cortex.silly/joint-dispatch
rlm@130:    {:type :point}
rlm@130:    (.getControl obj-a RigidBodyControl)
rlm@130:    (.getControl obj-b RigidBodyControl)
rlm@130:    (cortex.silly/world-to-local obj-a world-pivot)
rlm@130:    (cortex.silly/world-to-local obj-b world-pivot)
rlm@130:    nil
rlm@130:    ))
rlm@130: 
rlm@133: (import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@130: 
rlm@130: (defn blab-* []
rlm@130:   (let [hand   (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
rlm@130:                      :mass 0 :color ColorRGBA/Green)
rlm@130:         finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
rlm@130:                     :mass 1 :color ColorRGBA/Red)
rlm@130:         connection-point (Vector3f. 1.2 0 0)
rlm@130:         root (nodify [hand finger])]
rlm@130: 
rlm@130:     (join-at-point hand finger (Vector3f. 1.2 0 0))
rlm@130:     
rlm@130:     (.setCollisionGroup
rlm@130:      (.getControl hand RigidBodyControl)
rlm@130:      PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130:     (world
rlm@130:      root
rlm@130:      standard-debug-controls
rlm@130:      (fn [world]
rlm@130:        (enable-debug world)
rlm@130:        (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@130:        (set-gravity world Vector3f/ZERO)
rlm@130:        )
rlm@130:      no-op)))
rlm@131: (import java.awt.image.BufferedImage)
rlm@131: 
rlm@131: (defn draw-sprite [image sprite x y color ]
rlm@131:   (dorun
rlm@131:    (for [[u v] sprite]
rlm@131:      (.setRGB image (+ u x) (+ v y) color))))
rlm@131: 
rlm@131: (defn view-angle
rlm@132:   "create a debug view of an angle"
rlm@131:   [color]
rlm@131:   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@131:         previous (atom [25 25])
rlm@131:         sprite [[0 0] [0 1]
rlm@131:                 [0 -1] [-1 0] [1 0]]] 
rlm@131:     (fn [angle]
rlm@131:       (let [angle (float angle)]
rlm@131:         (let [position
rlm@131:               [(+ 25 (int (* 20 (Math/cos angle))))
rlm@131:                (+ 25 (int (* 20(Math/sin angle))))]]
rlm@131:           (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@131:           (draw-sprite image sprite (position 0) (position 1) color)
rlm@131:           (reset! previous position))
rlm@131:         image))))
rlm@131: 
rlm@130: (defn proprioception-debug-window
rlm@130:   []
rlm@131:   (let [yaw (view-angle 0xFF0000)
rlm@133:         roll (view-angle 0x00FF00)
rlm@133:         pitch (view-angle 0xFFFFFF)
rlm@131:         v-yaw (view-image)
rlm@133:         v-roll (view-image)
rlm@131:         v-pitch (view-image)
rlm@131:         ]
rlm@130:     (fn [prop-data]
rlm@130:       (dorun
rlm@130:        (map
rlm@133:         (fn [[y r p]]
rlm@131:           (v-yaw (yaw y))
rlm@131:           (v-roll (roll r))
rlm@131:           (v-pitch (pitch p)))
rlm@131:         prop-data)))))
rlm@133: (comment  
rlm@133:      
rlm@133: (defn proprioception-debug-window
rlm@133:   []
rlm@133:   (let [time (atom 0)]
rlm@133:     (fn [prop-data]
rlm@133:       (if (= 0 (rem (swap! time inc) 40))
rlm@133:         (println-repl prop-data)))))
rlm@133: )
rlm@133: 
rlm@131: (comment
rlm@131:   (dorun
rlm@131:    (map
rlm@131:     (comp 
rlm@131:      println-repl
rlm@131:      (fn [[p y r]]
rlm@131:        (format
rlm@131:         "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@131:         p y r)))
rlm@131:     prop-data)))
rlm@131: 
rlm@130: 
rlm@130: 
rlm@137: 
rlm@64: (defn test-proprioception
rlm@69:   "Testing proprioception:
rlm@69:    You should see two foating bars, and a printout of pitch, yaw, and
rlm@64:    roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@64:    change only the value of pitch. key-f/key-g moves it side to side
rlm@64:    and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@64:    and counterclockwise, and only affect roll."
rlm@60:   []
rlm@60:   (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
rlm@139:                      :mass 1 :color ColorRGBA/Green :name "hand")
rlm@60:         finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
rlm@132:                     :mass 1 :color ColorRGBA/Red :name "finger")
rlm@133:         joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@133:                         :position (Vector3f. 1.2 2 0)
rlm@133:                         :physical? false)
rlm@135:         joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
rlm@135:         creature (nodify [hand finger joint-node])
rlm@135:         ;; *******************************************
rlm@137:                 
rlm@135:         floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@135:                      :mass 0 :color ColorRGBA/Gray)
rlm@137:         
rlm@137:         root (nodify [creature floor])
rlm@133:         prop (joint-proprioception creature joint-node)
rlm@139:         prop-view (proprioception-debug-window)
rlm@139:         finger-control (.getControl finger RigidBodyControl)
rlm@139:         hand-control (.getControl hand RigidBodyControl)
rlm@139:         
rlm@139:         controls
rlm@139:         (merge standard-debug-controls
rlm@140:                {"key-o"
rlm@139:                 (fn [_ _] (.setEnabled finger-control true))
rlm@140:                 "key-p"
rlm@139:                 (fn [_ _] (.setEnabled finger-control false))
rlm@140:                 "key-k"
rlm@140:                 (fn [_ _] (.setEnabled hand-control true))
rlm@140:                 "key-l"
rlm@140:                 (fn [_ _] (.setEnabled hand-control false))
rlm@139:                 "key-i"
rlm@139:                 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@139:                 }
rlm@139:                )
rlm@130: 
rlm@139:         ]
rlm@139:     (comment
rlm@139:       (.setCollisionGroup
rlm@139:        (.getControl hand RigidBodyControl)
rlm@139:        PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@139:       )
rlm@140:     (apply
rlm@140:      world
rlm@140:      (with-movement
rlm@140:        hand
rlm@140:        ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@140:        [10 10 10 10 1 1]
rlm@140:        (with-movement
rlm@140:          finger
rlm@140:          ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@140:          [10 10 10 10 1 1]
rlm@140:          [root
rlm@140:           controls
rlm@140:           (fn [world]
rlm@140:             (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@140:             (set-gravity world (Vector3f. 0 0 0))
rlm@140:             (light-up-everything world))
rlm@140:           (fn [_ _] (prop-view (list (prop))))])))))
rlm@138: 
rlm@64: #+end_src
rlm@56: 
rlm@130: #+results: test-body
rlm@130: : #'cortex.test.body/test-proprioception
rlm@130: 
rlm@60: 
rlm@63: * COMMENT code-limbo
rlm@61: #+begin_src clojure
rlm@61: ;;(.loadModel 
rlm@61: ;; (doto (asset-manager)
rlm@61: ;;   (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61: ;;   "Models/person/person.blend")
rlm@61: 
rlm@64: 
rlm@64: (defn load-blender-model
rlm@64:   "Load a .blend file using an asset folder relative path."
rlm@64:   [^String model]
rlm@64:   (.loadModel
rlm@64:    (doto (asset-manager)
rlm@64:      (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@64:    model))
rlm@64: 
rlm@64: 
rlm@61: (defn view-model [^String model]
rlm@61:   (view 
rlm@61:    (.loadModel 
rlm@61:     (doto (asset-manager)
rlm@61:       (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61:     model)))
rlm@61: 
rlm@61: (defn load-blender-scene [^String model]
rlm@61:   (.loadModel
rlm@61:    (doto (asset-manager)
rlm@61:      (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@61:     model))
rlm@61: 
rlm@61: (defn worm
rlm@61:   []
rlm@61:   (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@61: 
rlm@61: (defn oto
rlm@61:   []
rlm@61:   (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@61: 
rlm@61: (defn sinbad
rlm@61:   []
rlm@61:   (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@61: 
rlm@61: (defn worm-blender
rlm@61:   []
rlm@61:   (first (seq (.getChildren (load-blender-model
rlm@61:                              "Models/anim2/simple-worm.blend")))))
rlm@61: 
rlm@61: (defn body
rlm@61:   "given a node with a SkeletonControl, will produce a body sutiable
rlm@61:   for AI control with movement and proprioception."
rlm@61:   [node]
rlm@61:   (let [skeleton-control (.getControl node SkeletonControl)
rlm@61:         krc (KinematicRagdollControl.)]
rlm@61:     (comment
rlm@61:     (dorun
rlm@61:      (map #(.addBoneName krc %)
rlm@61:           ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@61:            ;;"mid2" "mid3" "tail" "head"]
rlm@61:         )))
rlm@61:     (.addControl node krc)
rlm@61:     (.setRagdollMode krc)
rlm@61:     )
rlm@61:   node
rlm@61:   )
rlm@61: (defn show-skeleton [node]
rlm@61:   (let [sd
rlm@61: 
rlm@61:         (doto
rlm@61:             (SkeletonDebugger. "aurellem-skel-debug"
rlm@61:                                (skel node))
rlm@61:           (.setMaterial (green-x-ray)))]
rlm@61:     (.attachChild node sd)
rlm@61:     node))
rlm@61: 
rlm@61: 
rlm@61:   
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61: 
rlm@61: ;; this could be a good way to give objects special properties like
rlm@61: ;; being eyes and the like
rlm@61: 
rlm@61: (.getUserData 
rlm@61:  (.getChild
rlm@61:   (load-blender-model "Models/property/test.blend") 0)
rlm@61:  "properties")
rlm@61: 
rlm@61: ;; the properties are saved along with the blender file.
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61: 
rlm@61: 
rlm@61: 
rlm@61: 
rlm@61: (defn init-debug-skel-node
rlm@61:   [f debug-node skeleton]
rlm@61:   (let [bones 
rlm@61:         (map #(.getBone skeleton %)
rlm@61:              (range (.getBoneCount skeleton)))]
rlm@61:   (dorun (map #(.setUserControl % true) bones)) 
rlm@61:   (dorun (map (fn [b]
rlm@61:                 (println (.getName b)
rlm@61:                          " -- " (f b)))
rlm@61:               bones))
rlm@61:   (dorun
rlm@61:    (map #(.attachChild
rlm@61:           debug-node 
rlm@61:           (doto 
rlm@61:               (sphere 0.1
rlm@61:                       :position (f %)
rlm@61:                       :physical? false)
rlm@61:           (.setMaterial (green-x-ray))))
rlm@61:         bones)))
rlm@61:     debug-node)
rlm@61: 
rlm@61: (import jme3test.bullet.PhysicsTestHelper)
rlm@61: 
rlm@61: 
rlm@61: (defn test-zzz [the-worm world value]
rlm@61:   (if (not value)
rlm@61:     (let [skeleton (skel the-worm)]
rlm@61:       (println-repl "enabling bones")
rlm@61:       (dorun
rlm@61:        (map
rlm@61:         #(.setUserControl (.getBone skeleton %) true)
rlm@61:         (range (.getBoneCount skeleton))))
rlm@61: 
rlm@61: 
rlm@61:       (let [b (.getBone skeleton 2)]
rlm@61:         (println-repl "moving " (.getName b))
rlm@61:         (println-repl (.getLocalPosition b))
rlm@61:         (.setUserTransforms b
rlm@61:                             Vector3f/UNIT_X
rlm@61:                             Quaternion/IDENTITY
rlm@61:                             ;;(doto (Quaternion.)
rlm@61:                              ;; (.fromAngles (/ Math/PI 2)
rlm@61:                              ;;              0
rlm@61:                              ;;              0
rlm@61:                                           
rlm@61:                             (Vector3f. 1 1 1))
rlm@61:         )
rlm@61:       
rlm@61:       (println-repl "hi! <3"))))
rlm@61: 
rlm@61: 
rlm@61: (defn test-ragdoll []
rlm@61:   
rlm@61:   (let [the-worm
rlm@61:         
rlm@61:          ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@61:          (doto (show-skeleton (worm-blender))
rlm@61:            (.setLocalTranslation (Vector3f. 0 10 0))
rlm@61:          ;;(worm)
rlm@61:          ;;(oto)
rlm@61:          ;;(sinbad)
rlm@61:         )
rlm@61:     ]
rlm@61: 
rlm@61: 
rlm@61:     (.start 
rlm@61:      (world
rlm@61:       (doto (Node.)
rlm@61:         (.attachChild the-worm))
rlm@61:       {"key-return" (fire-cannon-ball)
rlm@61:        "key-space" (partial test-zzz the-worm)
rlm@61:        }
rlm@61:       (fn [world]
rlm@61:         (light-up-everything world)
rlm@61:         (PhysicsTestHelper/createPhysicsTestWorld
rlm@61:          (.getRootNode world)
rlm@61:          (asset-manager)
rlm@61:          (.getPhysicsSpace
rlm@61:           (.getState (.getStateManager world) BulletAppState)))
rlm@61:         (set-gravity world Vector3f/ZERO)
rlm@61:         ;;(.setTimer world (NanoTimer.))
rlm@61:         ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@61:         )
rlm@61:       no-op
rlm@61:       )
rlm@61:   
rlm@61:      
rlm@61:      )))
rlm@61: 
rlm@61: 
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61: ;;; here is the ragdoll stuff
rlm@61: 
rlm@61: (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@61: (def mesh worm-mesh)
rlm@61: 
rlm@61: (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@61: (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61: (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61: 
rlm@61: 
rlm@61: (defn position [index]
rlm@61:   (.get
rlm@61:    (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@61:      index))
rlm@61: 
rlm@61: (defn bones [index]
rlm@61:   (.get
rlm@61:    (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61:    index))
rlm@61: 
rlm@61: (defn bone-weights [index]
rlm@61:   (.get
rlm@61:    (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61:    index))
rlm@61: 
rlm@61: 
rlm@61: 
rlm@61: (defn vertex-bones [vertex]
rlm@61:   (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61: 
rlm@61: (defn vertex-weights [vertex]
rlm@61:   (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61: 
rlm@61: (defn vertex-position [index]
rlm@61:   (let [offset (* index 3)]
rlm@61:     (Vector3f. (position offset)
rlm@61:                (position (inc offset))
rlm@61:                (position (inc(inc offset))))))
rlm@61: 
rlm@61: (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@61: 
rlm@61: (defn bone-control-color [index]
rlm@61:   (get {[1 0 0 0] ColorRGBA/Red
rlm@61:         [1 2 0 0] ColorRGBA/Magenta
rlm@61:         [2 0 0 0] ColorRGBA/Blue}
rlm@61:        (vertex-bones index)
rlm@61:        ColorRGBA/White))
rlm@61: 
rlm@61: (defn influence-color [index bone-num]
rlm@61:   (get
rlm@61:    {(float 0)   ColorRGBA/Blue
rlm@61:     (float 0.5) ColorRGBA/Green
rlm@61:     (float 1)   ColorRGBA/Red}
rlm@61:      ;; find the weight of the desired bone
rlm@61:    ((zipmap (vertex-bones index)(vertex-weights index))
rlm@61:     bone-num)
rlm@61:    ColorRGBA/Blue))
rlm@61:          
rlm@61: (def worm-vertices (set (map vertex-info (range 60))))
rlm@61: 
rlm@61: 
rlm@61: (defn test-info []
rlm@61:   (let [points (Node.)]
rlm@61:     (dorun
rlm@61:      (map #(.attachChild points %)
rlm@61:           (map #(sphere 0.01
rlm@61:                         :position (vertex-position %)
rlm@61:                         :color (influence-color % 1)
rlm@61:                         :physical? false)
rlm@61:                (range 60))))
rlm@61:     (view points)))
rlm@61:   
rlm@61: 
rlm@61: (defrecord JointControl [joint physics-space]
rlm@61:   PhysicsControl
rlm@61:   (setPhysicsSpace [this space]
rlm@61:     (dosync
rlm@61:      (ref-set (:physics-space this) space))
rlm@61:     (.addJoint space (:joint this)))
rlm@61:   (update [this tpf])
rlm@61:   (setSpatial [this spatial])
rlm@61:   (render [this rm vp])
rlm@61:   (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@61:   (isEnabled [this] true)
rlm@61:   (setEnabled [this state]))
rlm@61:   
rlm@61: (defn add-joint
rlm@61:   "Add a joint to a particular object. When the object is added to the
rlm@61:   PhysicsSpace of a simulation, the joint will also be added"
rlm@61:   [object joint]
rlm@61:   (let [control (JointControl. joint (ref nil))]
rlm@61:     (.addControl object control))
rlm@61:   object)
rlm@61: 
rlm@61: 
rlm@61: (defn hinge-world
rlm@61:   []
rlm@61:   (let [sphere1 (sphere)
rlm@61:         sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@61:         joint (Point2PointJoint.
rlm@61:                (.getControl sphere1 RigidBodyControl)
rlm@61:                (.getControl sphere2 RigidBodyControl) 
rlm@61:                Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@61:     (add-joint sphere1 joint)
rlm@61:     (doto (Node. "hinge-world")
rlm@61:       (.attachChild sphere1)
rlm@61:       (.attachChild sphere2))))
rlm@61: 
rlm@61: 
rlm@61: (defn test-joint []
rlm@61:   (view (hinge-world)))
rlm@61: 
rlm@61: ;; (defn copier-gen []
rlm@61: ;;   (let [count (atom 0)]
rlm@61: ;;     (fn [in]
rlm@61: ;;       (swap! count inc)
rlm@61: ;;       (clojure.contrib.duck-streams/copy
rlm@61: ;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@61: ;;                      ;;/home/r/tmp/mao-test/clojure-images
rlm@61: ;;                (format "%08d.png" @count)))))))
rlm@61: ;; (defn decrease-framerate []
rlm@61: ;;   (map
rlm@61: ;;    (copier-gen)
rlm@61: ;;    (sort
rlm@61: ;;     (map first
rlm@61: ;;          (partition
rlm@61: ;;           4
rlm@61: ;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@61: ;;                   (file-seq
rlm@61: ;;                    (file-str
rlm@61: ;;                     "/home/r/media/anime/mao-temp/images"))))))))
rlm@61: 
rlm@61: 
rlm@61: 
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61: 
rlm@61: (defn proprioception
rlm@61:   "Create a proprioception map that reports the rotations of the
rlm@61:   various limbs of the creature's body"
rlm@61:   [creature]
rlm@61:   [#^Node creature]
rlm@61:   (let [
rlm@61:         nodes (node-seq creature)
rlm@61:         joints
rlm@61:         (map
rlm@61:          :joint 
rlm@61:          (filter
rlm@61:           #(isa? (class %) JointControl)
rlm@61:           (reduce
rlm@61:            concat
rlm@61:            (map (fn [node]
rlm@61:                   (map (fn [num] (.getControl node num))
rlm@61:                        (range (.getNumControls node))))
rlm@61:                 nodes))))]
rlm@61:     (fn []
rlm@61:       (reduce concat (map relative-positions (list (first joints)))))))
rlm@61: 
rlm@61: 
rlm@63: (defn skel [node]
rlm@63:   (doto
rlm@63:       (.getSkeleton
rlm@63:        (.getControl node SkeletonControl))
rlm@63:     ;; this is necessary to force the skeleton to have accurate world
rlm@63:     ;; transforms before it is rendered to the screen. 
rlm@63:     (.resetAndUpdate)))
rlm@63: 
rlm@63: (defn green-x-ray []
rlm@63:   (doto (Material. (asset-manager)
rlm@63:                    "Common/MatDefs/Misc/Unshaded.j3md")
rlm@63:     (.setColor "Color" ColorRGBA/Green)
rlm@63:     (-> (.getAdditionalRenderState)
rlm@63:         (.setDepthTest false))))
rlm@63: 
rlm@63: (defn test-worm []
rlm@63:   (.start
rlm@63:    (world
rlm@63:     (doto (Node.)
rlm@63:       ;;(.attachChild (point-worm))
rlm@63:       (.attachChild (load-blender-model
rlm@63:                      "Models/anim2/joint-worm.blend"))
rlm@63:                       
rlm@63:       (.attachChild (box 10 1 10
rlm@63:                          :position (Vector3f. 0 -2 0) :mass 0
rlm@63:                          :color (ColorRGBA/Gray))))
rlm@63:     {
rlm@63:      "key-space" (fire-cannon-ball)
rlm@63:      }
rlm@63:     (fn [world]
rlm@63:       (enable-debug world)
rlm@63:       (light-up-everything world)
rlm@63:       ;;(.setTimer world (NanoTimer.))
rlm@63:       )
rlm@63:     no-op)))
rlm@63: 
rlm@63: 
rlm@63: 
rlm@63: ;; defunct movement stuff
rlm@63: (defn torque-controls [control]
rlm@63:   (let [torques
rlm@63:         (concat
rlm@63:          (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@63:               (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@63:          [Vector3f/UNIT_X])]
rlm@63:     (map (fn [torque-axis]
rlm@63:            (fn [torque]
rlm@63:              (.applyTorque
rlm@63:               control
rlm@63:               (.mult (.mult (.getPhysicsRotation control)
rlm@63:                             torque-axis)
rlm@63:                      (float
rlm@63:                       (* (.getMass control) torque))))))
rlm@63:          torques)))
rlm@63: 
rlm@63: (defn motor-map
rlm@63:   "Take a creature and generate a function that will enable fine
rlm@63:    grained control over all the creature's limbs."  
rlm@63:   [#^Node creature]
rlm@63:   (let [controls (keep #(.getControl % RigidBodyControl)
rlm@63:                        (node-seq creature))
rlm@63:         limb-controls (reduce concat (map torque-controls controls))
rlm@63:         body-control  (partial map #(%1 %2) limb-controls)]
rlm@63:     body-control))
rlm@63: 
rlm@63: (defn test-motor-map
rlm@63:   "see how torque works."
rlm@63:   []
rlm@63:   (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@63:                     :mass 1 :color ColorRGBA/Green)
rlm@63:         motor-map (motor-map finger)]
rlm@63:     (world
rlm@63:      (nodify [finger
rlm@63:               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@63:                    :color ColorRGBA/Gray)])
rlm@63:      standard-debug-controls
rlm@63:      (fn [world]
rlm@63:        (set-gravity world Vector3f/ZERO)
rlm@63:        (light-up-everything world)
rlm@63:        (.setTimer world (NanoTimer.)))
rlm@63:      (fn [_ _]
rlm@63:        (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@61: #+end_src
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@0: 
rlm@73: * COMMENT generate Source
rlm@44: #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@64: <<proprioception>>
rlm@64: <<motor-control>>
rlm@0: #+end_src
rlm@64: 
rlm@69: #+begin_src clojure :tangle ../src/cortex/test/body.clj
rlm@64: <<test-body>>
rlm@64: #+end_src  
rlm@64: 
rlm@64: 
rlm@0: