#+title: The BODY!!!

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.

 #+SETUPFILE: ../../aurellem/org/setup.org

 #+INCLUDE: ../../aurellem/org/level-0.org

 

 * Proprioception

 #+name: proprioception

 #+begin_src clojure 

 (ns cortex.body

   (:use (cortex world util))

   (:import

    com.jme3.math.Vector3f

    com.jme3.math.Quaternion

    com.jme3.math.Vector2f

    com.jme3.math.Matrix3f

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.collision.CollisionResults

    com.jme3.bounding.BoundingBox))

 

 (import com.jme3.scene.Node)

 

 (defn jme-to-blender

   "Convert from JME coordinates to Blender coordinates"

   [#^Vector3f in]

   (Vector3f. (.getX in)

              (- (.getZ in))

              (.getY in)))

 

 (defn joint-targets

   "Return the two closest two objects to the joint object, ordered

   from bottom to top according to the joint's rotation."

   [#^Node parts #^Node joint]

   (loop [radius (float 0.01)]

     (let [results (CollisionResults.)]

       (.collideWith

        parts

        (BoundingBox. (.getWorldTranslation joint)

                      radius radius radius)

        results)

       (let [targets

             (distinct

              (map  #(.getGeometry %) results))]

         (if (>= (count targets) 2)

           (sort-by

            #(let [v

                   (jme-to-blender

                    (.mult

                     (.inverse (.getWorldRotation joint))

                     (.subtract (.getWorldTranslation %)

                                (.getWorldTranslation joint))))]

               (println-repl (.getName %) ":" v)

               (.dot (Vector3f. 1 1 1)

                     v))                  

            (take 2 targets))

           (recur (float (* radius 2))))))))

 

 (defn creature-joints 

   "Return the children of the creature's \"joints\" node."

   [#^Node creature]

   (if-let [joint-node (.getChild creature "joints")]

     (seq (.getChildren joint-node))

     (do (println-repl "could not find JOINTS node") [])))

 

 (defn right-handed? [vec1 vec2 vec3]

   (< 0 (.dot (.cross vec1 vec2) vec3)))

 

 (defn absolute-angle [vec1 vec2 axis]

   (let [angle (.angleBetween vec1 vec2)]

     (if (right-handed? vec1 vec2 axis)

       angle (- (* 2 Math/PI) angle))))

 

 

 (defn joint-proprioception [#^Node parts #^Node joint]

   (let [[obj-a obj-b] (joint-targets parts joint)

         joint-rot (.getWorldRotation joint)

         x0 (.mult joint-rot Vector3f/UNIT_X)

         y0 (.mult joint-rot Vector3f/UNIT_Y)

         z0 (.mult joint-rot Vector3f/UNIT_Z)]

     (println-repl "x:" x0)

     (println-repl "y:" y0)

     (println-repl "z:" z0)

     (println-repl "init-a:" (.getWorldRotation obj-a))

     (println-repl "init-b:" (.getWorldRotation obj-b))

 

     (fn []

       (let [rot-a (.clone (.getWorldRotation obj-a))

             rot-b (.clone (.getWorldRotation obj-b))

             x (.mult rot-a x0)

             y (.mult rot-a y0)

             z (.mult rot-a z0)

 

             X (.mult rot-b x0)

             Y (.mult rot-b y0)

             Z (.mult rot-b z0)

             heading  (Math/atan2 (.dot X z) (.dot X x))

             pitch  (Math/atan2 (.dot X y) (.dot X x))

 

             ;; rotate x-vector back to origin

             reverse

             (doto (Quaternion.)

               (.fromAngleAxis

                (.angleBetween X x)

                (let [cross (.normalize (.cross X x))]

                  (if (= 0 (.length cross)) y cross))))

             roll (absolute-angle (.mult reverse Y) y x)]

         

         [heading pitch roll]))))

 

 (defn proprioception

   "Create a function that provides proprioceptive information about an

   entire body."

   [#^Node creature]

   ;; extract the body's joints

   (let [joints (creature-joints creature)

         senses (map (partial joint-proprioception creature) joints)]

     (fn []

       (map #(%) senses))))

 

 (defn tap [obj direction force]

   (let [control (.getControl obj RigidBodyControl)]

     (.applyTorque

      control

      (.mult (.getPhysicsRotation control)

             (.mult (.normalize direction) (float force))))))

 

 

 (defn with-movement

   [object

    [up down left right roll-up roll-down :as keyboard]

    forces

    [root-node

     keymap

     intilization

     world-loop]]

   (let [add-keypress

         (fn [state keymap key]

           (merge keymap

                   {key

                    (fn [_ pressed?]

                      (reset! state pressed?))}))

         move-up? (atom false)

         move-down? (atom false)

         move-left? (atom false)

         move-right? (atom false)

         roll-left? (atom false)

         roll-right? (atom false)

         

         directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0)

                     (Vector3f. 0 0 1)(Vector3f. 0 0 -1)

                     (Vector3f. -1 0 0)(Vector3f. 1 0 0)]

         atoms [move-left? move-right? move-up? move-down? 

                  roll-left? roll-right?]

 

         keymap* (reduce merge

                         (map #(add-keypress %1 keymap %2)

                              atoms

                              keyboard))

         

         splice-loop (fn []

                       (dorun

                        (map

                         (fn [sym direction force]

                           (if @sym

                             (tap object direction force)))

                         atoms directions forces)))

 

         world-loop* (fn [world tpf]

                        (world-loop world tpf)

                        (splice-loop))]

     [root-node

      keymap*

      intilization

      world-loop*]))

 

 (import java.awt.image.BufferedImage)

 

 (defn draw-sprite [image sprite x y color ]

   (dorun

    (for [[u v] sprite]

      (.setRGB image (+ u x) (+ v y) color))))

 

 (defn view-angle

   "create a debug view of an angle"

   [color]

   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)

         previous (atom [25 25])

         sprite [[0 0] [0 1]

                 [0 -1] [-1 0] [1 0]]] 

     (fn [angle]

       (let [angle (float angle)]

         (let [position

               [(+ 25 (int (* 20 (Math/cos angle))))

                (+ 25 (int (* -20 (Math/sin angle))))]]

           (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)

           (draw-sprite image sprite (position 0) (position 1) color)

           (reset! previous position))

         image))))

 

 (defn proprioception-debug-window

   []

   (let [heading (view-angle 0xFF0000)

         pitch (view-angle 0x00FF00)

         roll (view-angle 0xFFFFFF)

         v-heading (view-image)

         v-pitch (view-image)

         v-roll (view-image)

         ]

     (fn [prop-data]

       (dorun

        (map

         (fn [[h p r]]

           (v-heading (heading h))

           (v-pitch (pitch p))

           (v-roll (roll r)))

         prop-data)))))

 

 

 #+end_src

 

 #+results: proprioception

 : #'cortex.body/proprioception-debug-window

 

 * Motor Control

 #+name: motor-control

 #+begin_src clojure

 (in-ns 'cortex.body)

 

 ;; surprisingly enough, terristerial creatures only move by using

 ;; torque applied about their joints.  There's not a single straight

 ;; line of force in the human body at all!  (A straight line of force

 ;; would correspond to some sort of jet or rocket propulseion.)

 

 (defn vector-motor-control

   "Create a function that accepts a sequence of Vector3f objects that

    describe the torque to be applied to each part of the body."

   [body]

   (let [nodes (node-seq body)

         controls (keep #(.getControl % RigidBodyControl) nodes)]

     (fn [torques]

       (map #(.applyTorque %1 %2)

            controls torques))))

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 #+end_src

  

 ## note -- might want to add a lower dimensional, discrete version of

 ## this if it proves useful from a x-modal clustering perspective.

 

 * Examples

 

 #+name: test-body

 #+begin_src clojure

 (ns cortex.test.body

   (:use (cortex world util body))

   (:require cortex.silly)

   (:import

    com.jme3.math.Vector3f

    com.jme3.math.ColorRGBA

    com.jme3.bullet.joints.Point2PointJoint

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.system.NanoTimer

    com.jme3.math.Quaternion))

 

 (defn worm-segments

   "Create multiple evenly spaced box segments. They're fabulous!"

   [segment-length num-segments interstitial-space radius]

   (letfn [(nth-segment

             [n]

             (box segment-length radius radius :mass 0.1

                  :position

                  (Vector3f.

                   (* 2 n (+ interstitial-space segment-length)) 0 0)

                  :name (str "worm-segment" n)

                  :color (ColorRGBA/randomColor)))]

     (map nth-segment (range num-segments))))

 

 (defn connect-at-midpoint

   "Connect two physics objects with a Point2Point joint constraint at

   the point equidistant from both objects' centers."

   [segmentA segmentB]

   (let [centerA (.getWorldTranslation segmentA)

         centerB (.getWorldTranslation segmentB)

         midpoint (.mult (.add centerA centerB) (float 0.5))

         pivotA (.subtract midpoint centerA)

         pivotB (.subtract midpoint centerB)

         

         ;; A side-effect of creating a joint registers

         ;; it with both physics objects which in turn

         ;; will register the joint with the physics system

         ;; when the simulation is started.

         joint (Point2PointJoint.

                (.getControl segmentA RigidBodyControl)

                (.getControl segmentB RigidBodyControl)

                pivotA

                pivotB)]

     segmentB))

 

 (defn eve-worm

   "Create a worm-like body bound by invisible joint constraints."

   []

   (let [segments (worm-segments 0.2 5 0.1 0.1)]

     (dorun (map (partial apply connect-at-midpoint)

                 (partition 2 1 segments)))

     (nodify "worm" segments)))

 

 (defn worm-pattern

   "This is a simple, mindless motor control pattern that drives the

    second segment of the worm's body at an offset angle with

    sinusoidally varying strength."

   [time]

   (let [angle (* Math/PI (/ 9 20))

         direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]

     [Vector3f/ZERO

      (.mult

       direction

       (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))

      Vector3f/ZERO

      Vector3f/ZERO

      Vector3f/ZERO]))

 

 (defn test-motor-control

   "Testing motor-control:

    You should see a multi-segmented worm-like object fall onto the

    table and begin writhing and moving."

   []

   (let [worm (eve-worm)

         time (atom 0)

         worm-motor-map (vector-motor-control worm)]

     (world

      (nodify [worm

               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0

                    :color ColorRGBA/Gray)])

      standard-debug-controls

      (fn [world]

        (enable-debug world)

        (light-up-everything world)

        (comment

          (com.aurellem.capture.Capture/captureVideo

           world

           (file-str "/home/r/proj/cortex/tmp/moving-worm")))

        )

      

      (fn [_ _]

        (swap! time inc)

        (Thread/sleep 20)

        (dorun (worm-motor-map

                (worm-pattern @time)))))))

 

 

 

 (defn join-at-point [obj-a obj-b world-pivot]

   (cortex.silly/joint-dispatch

    {:type :point}

    (.getControl obj-a RigidBodyControl)

    (.getControl obj-b RigidBodyControl)

    (cortex.silly/world-to-local obj-a world-pivot)

    (cortex.silly/world-to-local obj-b world-pivot)

    nil

    ))

 

 (import com.jme3.bullet.collision.PhysicsCollisionObject)

 

 (defn blab-* []

   (let [hand   (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)

                      :mass 0 :color ColorRGBA/Green)

         finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)

                     :mass 1 :color ColorRGBA/Red)

         connection-point (Vector3f. 1.2 0 0)

         root (nodify [hand finger])]

 

     (join-at-point hand finger (Vector3f. 1.2 0 0))

     

     (.setCollisionGroup

      (.getControl hand RigidBodyControl)

      PhysicsCollisionObject/COLLISION_GROUP_NONE)

     (world

      root

      standard-debug-controls

      (fn [world]

        (enable-debug world)

        (.setTimer world (com.aurellem.capture.RatchetTimer. 60))

        (set-gravity world Vector3f/ZERO)

        )

      no-op)))

 (comment  

      

 (defn proprioception-debug-window

   []

   (let [time (atom 0)]

     (fn [prop-data]

       (if (= 0 (rem (swap! time inc) 40))

         (println-repl prop-data)))))

 )

 

 (comment

   (dorun

    (map

     (comp 

      println-repl

      (fn [[p y r]]

        (format

         "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"

         p y r)))

     prop-data)))

 

 

 

 

 (defn test-proprioception

   "Testing proprioception:

    You should see two foating bars, and a printout of pitch, yaw, and

    roll. Pressing key-r/key-t should move the blue bar up and down and

    change only the value of pitch. key-f/key-g moves it side to side

    and changes yaw. key-v/key-b will spin the blue segment clockwise

    and counterclockwise, and only affect roll."

   []

   (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)

                      :mass 0 :color ColorRGBA/Green :name "hand")

         finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)

                     :mass 1 :color ColorRGBA/Red :name "finger")

         joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow

                         :position (Vector3f. 0 1.2 0)

                         :rotation (doto (Quaternion.)

                                     (.fromAngleAxis

                                      (/ Math/PI 2)

                                      (Vector3f. 0 0 1)))

                         :physical? false)

         joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))

         creature (nodify [hand finger joint-node])

         finger-control (.getControl finger RigidBodyControl)

         hand-control (.getControl hand RigidBodyControl)]

     

 

     (let

         ;; *******************************************

                 

         [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)

                      :mass 0 :color ColorRGBA/Gray)

         

         root (nodify [creature floor])

         prop (joint-proprioception creature joint-node)

         prop-view (proprioception-debug-window)

         

         controls

         (merge standard-debug-controls

                {"key-o"

                 (fn [_ _] (.setEnabled finger-control true))

                 "key-p"

                 (fn [_ _] (.setEnabled finger-control false))

                 "key-k"

                 (fn [_ _] (.setEnabled hand-control true))

                 "key-l"

                 (fn [_ _] (.setEnabled hand-control false))

                 "key-i"

                 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))

                 "key-period"

                 (fn [world _]

                   (.setEnabled finger-control false)

                   (.setEnabled hand-control false)

                   (.rotate creature (doto (Quaternion.)

                                       (.fromAngleAxis

                                        (float (/ Math/PI 15))

                                        (Vector3f. 0 0 -1))))

                                               

                   (.setEnabled finger-control true)

                   (.setEnabled hand-control true)

                   (set-gravity world (Vector3f. 0 0 0))

                   )

                 

                   

                 }

                )

 

         ]

     (comment

       (.setCollisionGroup

        (.getControl hand RigidBodyControl)

        PhysicsCollisionObject/COLLISION_GROUP_NONE)

       )

     (apply

      world

      (with-movement

        hand

        ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]

        [10 10 10 10 1 1]

        (with-movement

          finger

          ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]

          [1 1 10 10 10 10]

          [root

           controls

           (fn [world]

             (.setTimer world (com.aurellem.capture.RatchetTimer. 60))

             (set-gravity world (Vector3f. 0 0 0))

             (light-up-everything world))

           (fn [_ _] (prop-view (list (prop))))]))))))

 

 #+end_src

 

 #+results: test-body

 : #'cortex.test.body/test-proprioception

 

 

 * COMMENT code-limbo

 #+begin_src clojure

 ;;(.loadModel 

 ;; (doto (asset-manager)

 ;;   (.registerLoader BlenderModelLoader (into-array String ["blend"])))

 ;;   "Models/person/person.blend")

 

 

 (defn load-blender-model

   "Load a .blend file using an asset folder relative path."

   [^String model]

   (.loadModel

    (doto (asset-manager)

      (.registerLoader BlenderModelLoader (into-array String ["blend"])))

    model))

 

 

 (defn view-model [^String model]

   (view 

    (.loadModel 

     (doto (asset-manager)

       (.registerLoader BlenderModelLoader (into-array String ["blend"])))

     model)))

 

 (defn load-blender-scene [^String model]

   (.loadModel

    (doto (asset-manager)

      (.registerLoader BlenderLoader (into-array String ["blend"])))

     model))

 

 (defn worm

   []

   (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))

 

 (defn oto

   []

   (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))

 

 (defn sinbad

   []

   (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))

 

 (defn worm-blender

   []

   (first (seq (.getChildren (load-blender-model

                              "Models/anim2/simple-worm.blend")))))

 

 (defn body

   "given a node with a SkeletonControl, will produce a body sutiable

   for AI control with movement and proprioception."

   [node]

   (let [skeleton-control (.getControl node SkeletonControl)

         krc (KinematicRagdollControl.)]

     (comment

     (dorun

      (map #(.addBoneName krc %)

           ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]

            ;;"mid2" "mid3" "tail" "head"]

         )))

     (.addControl node krc)

     (.setRagdollMode krc)

     )

   node

   )

 (defn show-skeleton [node]

   (let [sd

 

         (doto

             (SkeletonDebugger. "aurellem-skel-debug"

                                (skel node))

           (.setMaterial (green-x-ray)))]

     (.attachChild node sd)

     node))

 

 

   

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 ;; this could be a good way to give objects special properties like

 ;; being eyes and the like

 

 (.getUserData 

  (.getChild

   (load-blender-model "Models/property/test.blend") 0)

  "properties")

 

 ;; the properties are saved along with the blender file.

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 

 

 

 (defn init-debug-skel-node

   [f debug-node skeleton]

   (let [bones 

         (map #(.getBone skeleton %)

              (range (.getBoneCount skeleton)))]

   (dorun (map #(.setUserControl % true) bones)) 

   (dorun (map (fn [b]

                 (println (.getName b)

                          " -- " (f b)))

               bones))

   (dorun

    (map #(.attachChild

           debug-node 

           (doto 

               (sphere 0.1

                       :position (f %)

                       :physical? false)

           (.setMaterial (green-x-ray))))

         bones)))

     debug-node)

 

 (import jme3test.bullet.PhysicsTestHelper)

 

 

 (defn test-zzz [the-worm world value]

   (if (not value)

     (let [skeleton (skel the-worm)]

       (println-repl "enabling bones")

       (dorun

        (map

         #(.setUserControl (.getBone skeleton %) true)

         (range (.getBoneCount skeleton))))

 

 

       (let [b (.getBone skeleton 2)]

         (println-repl "moving " (.getName b))

         (println-repl (.getLocalPosition b))

         (.setUserTransforms b

                             Vector3f/UNIT_X

                             Quaternion/IDENTITY

                             ;;(doto (Quaternion.)

                              ;; (.fromAngles (/ Math/PI 2)

                              ;;              0

                              ;;              0

                                           

                             (Vector3f. 1 1 1))

         )

       

       (println-repl "hi! <3"))))

 

 

 (defn test-ragdoll []

   

   (let [the-worm

         

          ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")

          (doto (show-skeleton (worm-blender))

            (.setLocalTranslation (Vector3f. 0 10 0))

          ;;(worm)

          ;;(oto)

          ;;(sinbad)

         )

     ]

 

 

     (.start 

      (world

       (doto (Node.)

         (.attachChild the-worm))

       {"key-return" (fire-cannon-ball)

        "key-space" (partial test-zzz the-worm)

        }

       (fn [world]

         (light-up-everything world)

         (PhysicsTestHelper/createPhysicsTestWorld

          (.getRootNode world)

          (asset-manager)

          (.getPhysicsSpace

           (.getState (.getStateManager world) BulletAppState)))

         (set-gravity world Vector3f/ZERO)

         ;;(.setTimer world (NanoTimer.))

         ;;(org.lwjgl.input.Mouse/setGrabbed false)

         )

       no-op

       )

   

      

      )))

 

 

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 ;;; here is the ragdoll stuff

 

 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))

 (def mesh worm-mesh)

 

 (.getFloatBuffer mesh VertexBuffer$Type/Position)

 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)

 (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))

 

 

 (defn position [index]

   (.get

    (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)

      index))

 

 (defn bones [index]

   (.get

    (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))

    index))

 

 (defn bone-weights [index]

   (.get

    (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)

    index))

 

 

 

 (defn vertex-bones [vertex]

   (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))

 

 (defn vertex-weights [vertex]

   (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))

 

 (defn vertex-position [index]

   (let [offset (* index 3)]

     (Vector3f. (position offset)

                (position (inc offset))

                (position (inc(inc offset))))))

 

 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))

 

 (defn bone-control-color [index]

   (get {[1 0 0 0] ColorRGBA/Red

         [1 2 0 0] ColorRGBA/Magenta

         [2 0 0 0] ColorRGBA/Blue}

        (vertex-bones index)

        ColorRGBA/White))

 

 (defn influence-color [index bone-num]

   (get

    {(float 0)   ColorRGBA/Blue

     (float 0.5) ColorRGBA/Green

     (float 1)   ColorRGBA/Red}

      ;; find the weight of the desired bone

    ((zipmap (vertex-bones index)(vertex-weights index))

     bone-num)

    ColorRGBA/Blue))

          

 (def worm-vertices (set (map vertex-info (range 60))))

 

 

 (defn test-info []

   (let [points (Node.)]

     (dorun

      (map #(.attachChild points %)

           (map #(sphere 0.01

                         :position (vertex-position %)

                         :color (influence-color % 1)

                         :physical? false)

                (range 60))))

     (view points)))

   

 

 (defrecord JointControl [joint physics-space]

   PhysicsControl

   (setPhysicsSpace [this space]

     (dosync

      (ref-set (:physics-space this) space))

     (.addJoint space (:joint this)))

   (update [this tpf])

   (setSpatial [this spatial])

   (render [this rm vp])

   (getPhysicsSpace [this] (deref (:physics-space this)))

   (isEnabled [this] true)

   (setEnabled [this state]))

   

 (defn add-joint

   "Add a joint to a particular object. When the object is added to the

   PhysicsSpace of a simulation, the joint will also be added"

   [object joint]

   (let [control (JointControl. joint (ref nil))]

     (.addControl object control))

   object)

 

 

 (defn hinge-world

   []

   (let [sphere1 (sphere)

         sphere2 (sphere 1 :position (Vector3f. 3 3 3))

         joint (Point2PointJoint.

                (.getControl sphere1 RigidBodyControl)

                (.getControl sphere2 RigidBodyControl) 

                Vector3f/ZERO (Vector3f. 3 3 3))]

     (add-joint sphere1 joint)

     (doto (Node. "hinge-world")

       (.attachChild sphere1)

       (.attachChild sphere2))))

 

 

 (defn test-joint []

   (view (hinge-world)))

 

 ;; (defn copier-gen []

 ;;   (let [count (atom 0)]

 ;;     (fn [in]

 ;;       (swap! count inc)

 ;;       (clojure.contrib.duck-streams/copy

 ;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"

 ;;                      ;;/home/r/tmp/mao-test/clojure-images

 ;;                (format "%08d.png" @count)))))))

 ;; (defn decrease-framerate []

 ;;   (map

 ;;    (copier-gen)

 ;;    (sort

 ;;     (map first

 ;;          (partition

 ;;           4

 ;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))

 ;;                   (file-seq

 ;;                    (file-str

 ;;                     "/home/r/media/anime/mao-temp/images"))))))))

 

 

 

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 (defn proprioception

   "Create a proprioception map that reports the rotations of the

   various limbs of the creature's body"

   [creature]

   [#^Node creature]

   (let [

         nodes (node-seq creature)

         joints

         (map

          :joint 

          (filter

           #(isa? (class %) JointControl)

           (reduce

            concat

            (map (fn [node]

                   (map (fn [num] (.getControl node num))

                        (range (.getNumControls node))))

                 nodes))))]

     (fn []

       (reduce concat (map relative-positions (list (first joints)))))))

 

 

 (defn skel [node]

   (doto

       (.getSkeleton

        (.getControl node SkeletonControl))

     ;; this is necessary to force the skeleton to have accurate world

     ;; transforms before it is rendered to the screen. 

     (.resetAndUpdate)))

 

 (defn green-x-ray []

   (doto (Material. (asset-manager)

                    "Common/MatDefs/Misc/Unshaded.j3md")

     (.setColor "Color" ColorRGBA/Green)

     (-> (.getAdditionalRenderState)

         (.setDepthTest false))))

 

 (defn test-worm []

   (.start

    (world

     (doto (Node.)

       ;;(.attachChild (point-worm))

       (.attachChild (load-blender-model

                      "Models/anim2/joint-worm.blend"))

                       

       (.attachChild (box 10 1 10

                          :position (Vector3f. 0 -2 0) :mass 0

                          :color (ColorRGBA/Gray))))

     {

      "key-space" (fire-cannon-ball)

      }

     (fn [world]

       (enable-debug world)

       (light-up-everything world)

       ;;(.setTimer world (NanoTimer.))

       )

     no-op)))

 

 

 

 ;; defunct movement stuff

 (defn torque-controls [control]

   (let [torques

         (concat

          (map #(Vector3f. 0 (Math/sin %) (Math/cos %))

               (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))

          [Vector3f/UNIT_X])]

     (map (fn [torque-axis]

            (fn [torque]

              (.applyTorque

               control

               (.mult (.mult (.getPhysicsRotation control)

                             torque-axis)

                      (float

                       (* (.getMass control) torque))))))

          torques)))

 

 (defn motor-map

   "Take a creature and generate a function that will enable fine

    grained control over all the creature's limbs."  

   [#^Node creature]

   (let [controls (keep #(.getControl % RigidBodyControl)

                        (node-seq creature))

         limb-controls (reduce concat (map torque-controls controls))

         body-control  (partial map #(%1 %2) limb-controls)]

     body-control))

 

 (defn test-motor-map

   "see how torque works."

   []

   (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)

                     :mass 1 :color ColorRGBA/Green)

         motor-map (motor-map finger)]

     (world

      (nodify [finger

               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0

                    :color ColorRGBA/Gray)])

      standard-debug-controls

      (fn [world]

        (set-gravity world Vector3f/ZERO)

        (light-up-everything world)

        (.setTimer world (NanoTimer.)))

      (fn [_ _]

        (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0

      0]))))))

 

 (defn joint-proprioception [#^Node parts #^Node joint]

   (let [[obj-a obj-b] (joint-targets parts joint)

         joint-rot (.getWorldRotation joint)

         pre-inv-a (.inverse (.getWorldRotation obj-a))

         x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))

         y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))

         z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))

 

         x Vector3f/UNIT_Y

         y Vector3f/UNIT_Z

         z Vector3f/UNIT_X

 

 

         tmp-rot-a (.getWorldRotation obj-a)]

     (println-repl "x:" (.mult tmp-rot-a x))

     (println-repl "y:" (.mult tmp-rot-a y))

     (println-repl "z:" (.mult tmp-rot-a z))

     (println-repl "rot-a" (.getWorldRotation obj-a))

     (println-repl "rot-b" (.getWorldRotation obj-b))

     (println-repl "joint-rot" joint-rot)

     ;; this function will report proprioceptive information for the

     ;; joint.

     (fn []

       ;; x is the "twist" axis, y and z are the "bend" axes

       (let [rot-a (.getWorldRotation obj-a)

             ;;inv-a (.inverse rot-a)

             rot-b (.getWorldRotation obj-b)

             ;;relative (.mult rot-b inv-a)

             basis (doto (Matrix3f.)

                     (.setColumn 0 (.mult rot-a x))

                     (.setColumn 1 (.mult rot-a y))

                     (.setColumn 2 (.mult rot-a z)))

             rotation-about-joint

             (doto (Quaternion.)

               (.fromRotationMatrix 

                (.mult (.invert basis)

                       (.toRotationMatrix rot-b))))

             [yaw roll pitch]

             (seq (.toAngles rotation-about-joint nil))]

         ;;return euler angles of the quaternion around the new basis            

         [yaw roll pitch]))))

 

 #+end_src

 

 

 

 

 

 

 

 * COMMENT generate Source

 #+begin_src clojure :tangle ../src/cortex/body.clj

 <<proprioception>>

 <<motor-control>>

 #+end_src

 

 #+begin_src clojure :tangle ../src/cortex/test/body.clj

 <<test-body>>

 #+end_src