#+title: Building a 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

 

 

 * Design Constraints

 

 I use [[www.blender.org/][blender]] to design bodies.  The design of the bodies is

 determined by the requirements of the AI that will use them. The

 bodies must be easy for an AI to sense and control, and they must be

 relatively simple for jMonkeyEngine to compute.  

 

 ** Bag of Bones

 

 How to create such a body? One option I ultimately rejected is to use

 blender's [[http://wiki.blender.org/index.php/Doc:2.6/Manual/Rigging/Armatures][armature]] system. The idea would have been to define a mesh

 which describes the creature's entire body. To this you add an

 (skeleton) which deforms this mesh. This technique is used extensively

 to model humans and create realistic animations. It is hard to use for

 my purposes because it is difficult to update the creature's Physics

 Collision Mesh in tandem with its Geometric Mesh under the influence

 of the armature. Withouth this the creature will not be able to grab

 things in its environment, and it won't be able to tell where its

 physical body is by using its eyes. Also, armatures do not specify

 any rotational limits for a joint, making it hard to model elbows,

 shoulders, etc.

 

 ** EVE

 

 Instead of using the human-like "deformable bag of bones" approach, I

 decided to base my body plans on the robot EVE from the movie wall-E. 

 

 #+caption: EVE from the movie WALL-E.  This body plan turns out to be much better suited to my purposes than a more human-like one.

 [[../images/Eve.jpg]]

 

 The main reason that I use eve-style bodies is so that there will be

 correspondence between the AI's vision and the physical presence of

 its body. 

 

 * Solidifying the Body

 

 Here is a hand designed eve-style in blender.

 

 

 #+attr_html: width="500"

 [[../images/hand-screenshot0.png]]

 

 If we load it directly into jMonkeyEngine, we get this:

 

 #+name: test-0

 #+begin_src clojure

 (ns cortex.test.body

   (:use (cortex world util body))

   (:import (com.aurellem.capture Capture RatchetTimer)

            (com.jme3.math Quaternion Vector3f)

            java.io.File))

 

 (def hand-path "Models/test-creature/hand.blend")

 

 (defn hand [] (load-blender-model hand-path))

 

 (defn setup [world]

   (let [cam (.getCamera world)]

     (println-repl cam)

     (.setLocation

      cam (Vector3f. 

           -6.9015837, 8.644911, 5.6043186))

     (.setRotation

      cam

      (Quaternion.

       0.14046453, 0.85894054, -0.34301838, 0.3533118)))

   (light-up-everything world)

   (.setTimer world (RatchetTimer. 60))

   world)

 

 (defn test-one []

   (world (hand)

          standard-debug-controls

          (comp

           #(Capture/captureVideo

             % (File. "/home/r/proj/cortex/render/body/1"))

           setup)

          no-op))

 #+end_src

 

 

 #+begin_src clojure :results silent

 (.start (cortex.test.body/test-one))

 #+end_src

 

 #+begin_html

 <video controls="controls" width="755">

   <source src="../video/ghost-hand.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 #+end_html

 

 You will notice that the hand has no physical presence -- it's a

 hologram through witch everything passes.  Therefore, the first thing

 to do is to make it solid.  Blender has physics simulation on par with

 jMonkeyEngine (they both use bullet as their physics backend), but it

 can be difficult to translate between the two systems, so for now I

 specify the mass of each object in blender and construct the physics

 shape based on the mesh in jMonkeyEngine.

 

 #+name: joints-1

 #+begin_src clojure

 (defn physical!

   "Iterate through the nodes in creature and make them real physical

    objects in the simulation."

   [#^Node creature]

   (dorun

    (map

     (fn [geom]

       (let [physics-control

             (RigidBodyControl.

              (HullCollisionShape.

               (.getMesh geom))

              (if-let [mass (meta-data geom "mass")]

                (do

                  (println-repl

                   "setting" (.getName geom) "mass to" (float mass))

                  (float mass))

                (float 1)))]

         (.addControl geom physics-control)))

     (filter #(isa? (class %) Geometry )

             (node-seq creature)))))

 #+end_src

 

 =(physical!)= iterates through a creature's node structure, creating

 CollisionShapes for each geometry with the mass specified in that

 geometry's meta-data.

 

 #+begin_src clojure 

 (in-ns 'cortex.test.body)

 

 (def normal-gravity 

   {"key-g" (fn [world _]

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

 

 (defn floor [] 

   (box 10 3 10 :position (Vector3f. 0 -10 0)

        :color ColorRGBA/Gray :mass 0))

 

 (defn test-two []

   (world (nodify

           [(doto (hand)

              (physical!))

           (floor)])

          (merge standard-debug-controls normal-gravity)

          (comp

           #(Capture/captureVideo

             % (File. "/home/r/proj/cortex/render/body/2"))

           #(do (set-gravity % Vector3f/ZERO) %)

           setup)

          no-op))

 #+end_src

 

 #+begin_html

 <video controls="controls" width="755">

   <source src="../video/crumbly-hand.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 #+end_html

 

 Now that's some progress.

 

 

 * Joints

 

 Obviously, an AI is not going to be doing much just lying in pieces on

 the floor.  So, the next step to making a proper body is to connect

 those pieces together with joints.  jMonkeyEngine has a large array of

 joints available via bullet, such as Point2Point, Cone, Hinge, and a

 generic Six Degree of Freedom joint, with or without spring

 restitution.

 

 Although it should be possible to specify the joints using blender's

 physics system, and then automatically import them with jMonkeyEngine,

 the support isn't there yet, and there are a few problems with bullet

 itself that need to be solved before it can happen. 

 

 So, I will use the same system for specifying joints as I will do for

 some senses.  Each joint is specified by an empty node whose parent

 has the name "joints". Their orientation and meta-data determine what

 joint is created.

 

 [[../images/hand-screenshot1.png]]

 

 

 

 

 #+name: joints-2

 #+begin_src clojure 

 (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))))))))

 

 (defmulti joint-dispatch

   "Translate blender pseudo-joints into real JME joints."

   (fn [constraints & _] 

     (:type constraints)))

 

 (defmethod joint-dispatch :point

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating POINT2POINT joint")

   ;; bullet's point2point joints are BROKEN, so we must use the

   ;; generic 6DOF joint instead of an actual Point2Point joint!

 

   ;; should be able to do this:

   (comment 

     (Point2PointJoint.

      control-a

      control-b

      pivot-a

      pivot-b))

 

   ;; but instead we must do this:

   (println-repl "substuting 6DOF joint for POINT2POINT joint!")

   (doto

       (SixDofJoint.

        control-a

        control-b

        pivot-a

        pivot-b

        false)

     (.setLinearLowerLimit Vector3f/ZERO)

     (.setLinearUpperLimit Vector3f/ZERO)

     ;;(.setAngularLowerLimit (Vector3f. 1 1 1))

     ;;(.setAngularUpperLimit (Vector3f. 0 0 0))

 

 ))

 

 

 (defmethod joint-dispatch :hinge

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating HINGE joint")

   (let [axis

         (if-let

             [axis (:axis constraints)]

           axis

           Vector3f/UNIT_X)

         [limit-1 limit-2] (:limit constraints)

         hinge-axis

         (.mult

          rotation

          (blender-to-jme axis))]

     (doto

         (HingeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          hinge-axis

          hinge-axis)

       (.setLimit limit-1 limit-2))))

 

 (defmethod joint-dispatch :cone

   [constraints control-a control-b pivot-a pivot-b rotation]

   (let [limit-xz (:limit-xz constraints)

         limit-xy (:limit-xy constraints)

         twist    (:twist constraints)]

     

     (println-repl "creating CONE joint")

     (println-repl rotation)

     (println-repl

      "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))

     (println-repl

      "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))

     (println-repl

      "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))

     (doto

         (ConeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          rotation

          rotation)

       (.setLimit (float limit-xz)

                  (float limit-xy)

                  (float twist)))))

 

 (defn connect

   "Create a joint between 'obj-a and 'obj-b at the location of

   'joint. The type of joint is determined by the metadata on 'joint.

 

    Here are some examples:

    {:type :point}

    {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}

    (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)

 

    {:type :cone :limit-xz 0]

                 :limit-xy 0]

                 :twist 0]}   (use XZY rotation mode in blender!)"

   [#^Node obj-a #^Node obj-b #^Node joint]

   (let [control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         joint-center (.getWorldTranslation joint)

         joint-rotation (.toRotationMatrix (.getWorldRotation joint))

         pivot-a (world-to-local obj-a joint-center)

         pivot-b (world-to-local obj-b joint-center)]

    

     (if-let [constraints

              (map-vals

               eval

               (read-string

                (meta-data joint "joint")))]

       ;; 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.

       (do

         (println-repl "creating joint between"

                       (.getName obj-a) "and" (.getName obj-b))

         (joint-dispatch constraints

                         control-a control-b

                         pivot-a pivot-b

                         joint-rotation))

       (println-repl "could not find joint meta-data!"))))

 

 (defvar 

   ^{:arglists '([creature])}

   joints

   (sense-nodes "joints")

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

 

 (defn joints!

   "Connect the solid parts of the creature with physical joints. The

    joints are taken from the \"joints\" node in the creature."

   [#^Node creature]

   (dorun

    (map

     (fn [joint]

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

         (connect obj-a obj-b joint)))

     (joints creature))))

 

 (defn body!

   "Endow the creature with a physical body connected with joints.  The

    particulars of the joints and the masses of each pody part are

    determined in blender."

   [#^Node creature]

   (physical! creature)

   (joints! creature))

 #+end_src

 

 * Bookkeeping

 

 #+name: body-0

 #+begin_src clojure

 (ns cortex.body

   "Assemble a physical creature using the definitions found in a

    specially prepared blender file. Creates rigid bodies and joints so

    that a creature can have a physical presense in the simulation."

   {:author "Robert McIntyre"}

   (:use (cortex world util sense))

   (:use clojure.contrib.def)

   (:import

    (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)

    (com.jme3.bullet.joints

     SixDofJoint Point2PointJoint HingeJoint ConeJoint)

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.collision.CollisionResults

    com.jme3.bounding.BoundingBox

    com.jme3.scene.Node

    com.jme3.scene.Geometry

    com.jme3.bullet.collision.shapes.HullCollisionShape))

 #+end_src

 

 * Source 

 

 * COMMENT 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

 <<joints>>

 #+end_src

 

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

 <<test-0>>

 #+end_src