rlm@73: #+title: First attempt at a creature!
rlm@73: #+author: Robert McIntyre
rlm@73: #+email: rlm@mit.edu
rlm@73: #+description: 
rlm@73: #+keywords: simulation, jMonkeyEngine3, clojure
rlm@73: #+SETUPFILE: ../../aurellem/org/setup.org
rlm@73: #+INCLUDE: ../../aurellem/org/level-0.org
rlm@73: 
rlm@73: * Intro 
rlm@73: So far, I've made the following senses -- 
rlm@73:  - Vision
rlm@73:  - Hearing
rlm@73:  - Touch
rlm@73:  - Proprioception
rlm@73: 
rlm@73: And one effector: 
rlm@73:  - Movement
rlm@73: 
rlm@73: However, the code so far has only enabled these senses, but has not
rlm@73: actually implemented them.  For example, there is still a lot of work
rlm@73: to be done for vision. I need to be able to create an /eyeball/ in
rlm@73: simulation that can be moved around and see the world from different
rlm@73: angles. I also need to determine weather to use log-polar or cartesian
rlm@73: for the visual input, and I need to determine how/wether to
rlm@73: disceritise the visual input.
rlm@73: 
rlm@73: I also want to be able to visualize both the sensors and the
rlm@73: effectors in pretty pictures. This semi-retarted creature will by my
rlm@73: first attempt at bringing everything together.
rlm@73: 
rlm@73: * The creature's body
rlm@73: 
rlm@73: Still going to do an eve-like body in blender, but due to problems
rlm@73: importing the joints, etc into jMonkeyEngine3, I',m going to do all
rlm@73: the connecting here in clojure code, using the names of the individual
rlm@73: components and trial and error. Later, I'll maybe make some sort of
rlm@73: creature-building modifications to blender that support whatever
rlm@73: discreitized senses I'm going to make.
rlm@73: 
rlm@73: #+name: body-1
rlm@73: #+begin_src clojure 
rlm@73: (ns cortex.silly
rlm@73:   "let's play!"
rlm@73:   {:author "Robert McIntyre"})
rlm@73: 
rlm@73: ;; TODO remove this!
rlm@73: (require 'cortex.import)
rlm@73: (cortex.import/mega-import-jme3)
rlm@73: (use '(cortex world util body hearing touch vision))
rlm@73: 
rlm@73: (rlm.rlm-commands/help)
rlm@73: 
rlm@83: (declare joint-create get-subjective-position)
rlm@83: 
rlm@83: (defn load-bullet []
rlm@83:   (.start (world (Node.) {} no-op no-op)))
rlm@83: 
rlm@73: (defn load-blender-model
rlm@73:   "Load a .blend file using an asset folder relative path."
rlm@73:   [^String model]
rlm@73:   (.loadModel
rlm@73:    (doto (asset-manager)
rlm@73:      (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@73:    model))
rlm@73: 
rlm@74: (defn meta-data [blender-node key]
rlm@74:   (if-let [data (.getUserData blender-node "properties")]
rlm@74:     (.findValue data key)
rlm@74:     nil))
rlm@73: 
rlm@78: (defn blender-to-jme
rlm@78:   "Convert from Blender coordinates to JME coordinates"
rlm@78:   [#^Vector3f in]
rlm@78:   (Vector3f. (.getX in)
rlm@78:              (.getZ in)
rlm@78:              (- (.getY in))))
rlm@74: 
rlm@79: (defn jme-to-blender
rlm@79:   "Convert from JME coordinates to Blender coordinates"
rlm@79:   [#^Vector3f in]
rlm@79:   (Vector3f. (.getX in)
rlm@79:              (- (.getZ in))
rlm@79:              (.getY in)))
rlm@79: 
rlm@78: (defn joint-targets
rlm@78:   "Return the two closest two objects to the joint object, ordered
rlm@78:   from bottom to top according to the joint's rotation."
rlm@78:   [#^Node parts #^Node joint]
rlm@78:   ;;(println (meta-data joint "joint"))
rlm@78:   (.getWorldRotation joint)
rlm@78:   (loop [radius (float 0.01)]
rlm@78:     (let [results (CollisionResults.)]
rlm@78:       (.collideWith
rlm@78:        parts
rlm@78:        (BoundingBox. (.getWorldTranslation joint)
rlm@78:                      radius radius radius)
rlm@78:        results)
rlm@78:       (let [targets
rlm@78:             (distinct
rlm@78:              (map  #(.getGeometry %) results))]
rlm@78:         (if (>= (count targets) 2)
rlm@78:           (sort-by
rlm@79:            #(let [v
rlm@79:                   (jme-to-blender
rlm@79:                    (.mult
rlm@79:                     (.inverse (.getWorldRotation joint))
rlm@79:                     (.subtract (.getWorldTranslation %)
rlm@79:                                (.getWorldTranslation joint))))]
rlm@79:               (println-repl (.getName %) ":" v)
rlm@79:               (.dot (Vector3f. 1 1 1)
rlm@79:                     v))                  
rlm@78:            (take 2 targets))
rlm@78:           (recur (float (* radius 2))))))))
rlm@74: 
rlm@78: (defn connect
rlm@78:   "here are some examples:
rlm@78:    {:type :point}
rlm@78:    {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
rlm@78:    (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
rlm@74: 
rlm@78:    {:type :cone   :limit-xz 0]
rlm@79:                   :limit-xy 0]
rlm@79:                   :twist 0]}   (use XZY rotation mode in blender!)"
rlm@81:   [#^Node obj-a #^Node obj-b #^Node joint]
rlm@81:   (let [center-a (.getWorldTranslation obj-a)
rlm@81:         center-b (.getWorldTranslation obj-b)
rlm@81:         joint-center (.getWorldTranslation joint)
rlm@81:         pivot-a (.subtract joint-center center-a)
rlm@81:         pivot-b (.subtract joint-center center-b)
rlm@81:         control-a (.getControl obj-a RigidBodyControl)
rlm@81:         control-b (.getControl obj-b RigidBodyControl)]
rlm@81:     ;; A side-effect of creating a joint registers
rlm@81:     ;; it with both physics objects which in turn
rlm@81:     ;; will register the joint with the physics system
rlm@81:     ;; when the simulation is started.
rlm@81:     (if-let [constraints
rlm@81:              (map-vals
rlm@81:               eval
rlm@81:               (read-string
rlm@81:                (meta-data joint "joint")))]
rlm@74: 
rlm@81:       (let [joint-type (:type constraints)]
rlm@81:         (println-repl "creating joint between"
rlm@81:                       (.getName obj-a) "and" (.getName obj-b))
rlm@81:         (cond (= :point joint-type)
rlm@81:               (do
rlm@81:                 (println-repl "creating POINT joint")
rlm@81:                 (Point2PointJoint.
rlm@81:                  control-a
rlm@81:                  control-b
rlm@81:                  pivot-a
rlm@81:                  pivot-b))
rlm@81:               (= :hinge joint-type)
rlm@81:               (do
rlm@81:                 (println-repl "creating HINGE joint")
rlm@81:                 (let [axis (if-let
rlm@81:                                [axis (:axis constraints)]
rlm@81:                              axis
rlm@81:                              Vector3f/UNIT_X)
rlm@81:                       [limit-1 limit-2] (:limit constraints)
rlm@81:                       hinge-axis
rlm@81:                       (.mult
rlm@81:                        (.getWorldRotation joint)
rlm@81:                        (blender-to-jme axis))]
rlm@81:                   (doto
rlm@81:                       (HingeJoint.
rlm@81:                        control-a
rlm@81:                        control-b
rlm@81:                        pivot-a
rlm@81:                        pivot-b
rlm@81:                        hinge-axis
rlm@81:                        hinge-axis)
rlm@81:                     (.setLimit limit-1 limit-2))))
rlm@81:               (= :cone joint-type)
rlm@81:               (do
rlm@81:                 (let [limit-xz (:limit-xz constraints)
rlm@81:                       limit-xy (:limit-xy constraints)
rlm@81:                       twist    (:twist constraints)]
rlm@81:                   
rlm@81:                   (println-repl "creating CONE joint")
rlm@82:                   (let [frame-a
rlm@82:                         (.toRotationMatrix
rlm@82:                          (doto (Quaternion.)
rlm@82:                            (.fromAngleAxis
rlm@82:                             (float
rlm@82:                              (.angleBetween
rlm@83:                               (.normalize pivot-a) Vector3f/UNIT_X))
rlm@83:                             (.normalize
rlm@83:                              (.cross pivot-a
rlm@83:                                      Vector3f/UNIT_X)))))
rlm@83:                          ]
rlm@83:                     (println-repl "pivot-a" pivot-a)
rlm@81:                     (println-repl
rlm@83:                      "angle between pivot-a and UNIT_X is"
rlm@82:                      (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))
rlm@83:                     (println-repl "frame-a:" frame-a)
rlm@83:                     (println-repl
rlm@83:                      "frame-a moves Vector3f/UNIT_X to"
rlm@83:                      (.mult frame-a Vector3f/UNIT_X ))
rlm@81:                     
rlm@81:                     
rlm@81:                     (doto
rlm@81:                         (ConeJoint.
rlm@81:                          control-a
rlm@81:                          control-b
rlm@81:                          pivot-a
rlm@81:                          pivot-b
rlm@82: 
rlm@82:                         
rlm@83:                          ;; frame-in-A
rlm@83:                          ;;frame-a
rlm@83:                          ;;frame-a
rlm@81:                          
rlm@83:                          (.toRotationMatrix
rlm@83:                           (doto (Quaternion.)
rlm@83:                             (.fromAngles
rlm@83:                              0 0 (* -0.5 (/ Math/PI 2)))))
rlm@81:                          
rlm@83:                          
rlm@83:                          ;; frame-in-B
rlm@83:                          (.toRotationMatrix
rlm@83:                           (doto (Quaternion.)
rlm@83:                             (.fromAngles
rlm@83:                              0 0 (* -0.5 (/ Math/PI 2)))))
rlm@83:                          
rlm@82: 
rlm@81:                          )
rlm@81:                       (.setLimit (float limit-xz)
rlm@81:                                  (float limit-xy)
rlm@81:                                  (float twist))))))
rlm@81:               true
rlm@81:               (println-repl
rlm@81:                "joint-type" joint-type "not recognized")))
rlm@81:       
rlm@81:       (println-repl "could not find joint meta-data!"))))
rlm@74: 
rlm@83: 
rlm@78: (defn assemble-creature [#^Node pieces joints]
rlm@78:   (dorun
rlm@78:    (map
rlm@78:     (fn [geom]
rlm@78:       (let [physics-control
rlm@78:             (RigidBodyControl.
rlm@78:              (HullCollisionShape.
rlm@78:               (.getMesh geom))
rlm@78:              (if-let [mass (meta-data geom "mass")]
rlm@78:                (do
rlm@78:                  (println-repl
rlm@78:                   "setting" (.getName geom) "mass to" (float mass))
rlm@78:                  (float mass))
rlm@78:                (float 1)))]
rlm@78:                
rlm@78:         (.addControl geom physics-control)))
rlm@78:     (filter #(isa? (class %) Geometry )
rlm@78:             (node-seq pieces))))
rlm@77: 
rlm@78:   (dorun
rlm@78:    (map
rlm@78:     (fn [joint]
rlm@78:       (let [[obj-a obj-b]
rlm@78:             (joint-targets pieces joint)]
rlm@78:         (connect obj-a obj-b joint)))
rlm@78:     joints))
rlm@78:   pieces)
rlm@74: 
rlm@78: (defn blender-creature [blender-path]
rlm@78:   (let [model (load-blender-model blender-path)
rlm@78:         joints
rlm@78:         (if-let [joint-node (.getChild model "joints")]
rlm@78:           (seq (.getChildren joint-node))
rlm@78:           (do (println-repl "could not find joints node")
rlm@78:               []))]
rlm@78:   (assemble-creature model joints)))
rlm@74: 
rlm@78: (def hand "Models/creature1/one.blend")
rlm@74: 
rlm@78: (def worm "Models/creature1/try-again.blend")
rlm@78: 
rlm@80: (defn x-ray [#^ColorRGBA color]
rlm@80:   (doto (Material. (asset-manager)
rlm@80:                    "Common/MatDefs/Misc/Unshaded.j3md")
rlm@80:     (.setColor "Color" color)
rlm@80:     (-> (.getAdditionalRenderState)
rlm@80:         (.setDepthTest false))))
rlm@80: 
rlm@78: (defn test-creature [thing]
rlm@80:   (let [x-axis
rlm@80:         (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red)
rlm@80:         y-axis
rlm@80:         (box 0.01 1 0.01 :physical? false :color ColorRGBA/Green)
rlm@80:         z-axis
rlm@80:         (box 0.01 0.01 1 :physical? false :color ColorRGBA/Blue)]
rlm@78:   (world
rlm@78:    (nodify [(blender-creature thing)
rlm@81:             (box 10 2 10 :position (Vector3f. 0 -9 0)
rlm@80:                  :color ColorRGBA/Gray :mass 0)
rlm@80:             x-axis y-axis z-axis
rlm@80:             ])
rlm@78:    standard-debug-controls
rlm@78:    (comp light-up-everything enable-debug
rlm@78:          (fn [world]
rlm@78:            (.setTimer world (NanoTimer.))
rlm@80:            (set-gravity world (Vector3f. 0 0 0))
rlm@78:            (speed-up world)
rlm@78:            world
rlm@78:            ))
rlm@80:    no-op)))
rlm@78: 
rlm@78: (defn world-setup [joint]
rlm@83:   (let [
rlm@83: 
rlm@83:         joint-position (Vector3f. 0 4 0)
rlm@83:         joint-rotation
rlm@83:         (.toRotationMatrix
rlm@83:          (.mult
rlm@83:           (doto (Quaternion.)
rlm@83:             (.fromAngleAxis
rlm@83:              (* 1 (/ Math/PI 4))
rlm@83:              (Vector3f. -1 0 0)))
rlm@83:           (doto (Quaternion.)
rlm@83:             (.fromAngleAxis
rlm@83:              (/ Math/PI 2)
rlm@83:              (Vector3f. 0 0 1)))))
rlm@83:         
rlm@83:         origin (doto
rlm@83:                    (sphere 0.1 :physical? false :color ColorRGBA/Cyan
rlm@83:                            :position (Vector3f. 0 0 0)))
rlm@83:         top (doto
rlm@78:                 (sphere 0.1 :physical? false :color ColorRGBA/Yellow
rlm@83:                         :position (.mult joint-rotation (Vector3f. 8 0 0)))
rlm@83: 
rlm@78:               (.addControl
rlm@78:                (RigidBodyControl.
rlm@83:                 (CapsuleCollisionShape. 0.5 1.5 1) (float 20))))
rlm@78:         bottom (doto
rlm@78:                    (sphere 0.1 :physical? false :color ColorRGBA/DarkGray
rlm@83:                            :position (Vector3f. 0 0 0))
rlm@83:                  (.addControl
rlm@78:                (RigidBodyControl.
rlm@78:                 (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))
rlm@83:         table (box 10 2 10 :position (Vector3f. 0 -20 0)
rlm@78:                    :color ColorRGBA/Gray :mass 0)
rlm@78:         a (.getControl top RigidBodyControl)
rlm@78:         b (.getControl bottom RigidBodyControl)]
rlm@83: 
rlm@78:     (cond
rlm@83:      (= joint :cone)
rlm@83:      
rlm@83:        (doto (ConeJoint.
rlm@83:               a b
rlm@83:               (get-subjective-position joint-position top)
rlm@83:               (get-subjective-position joint-position bottom)
rlm@83:               joint-rotation
rlm@83:               joint-rotation
rlm@83:               )
rlm@78: 
rlm@83:               
rlm@83:          (.setLimit (* (/ 10) Math/PI)
rlm@83:                     (* (/ 4)  Math/PI)
rlm@83:                     0)))
rlm@83:   [origin top bottom table]))
rlm@78:             
rlm@78: 
rlm@78: 
rlm@78: (defn test-joint [joint]
rlm@83:   (let [[origin top bottom floor] (world-setup joint)
rlm@78:         control (.getControl top RigidBodyControl)
rlm@78:         move-up? (atom false)
rlm@78:         move-down? (atom false)
rlm@78:         move-left? (atom false)
rlm@78:         move-right? (atom false)
rlm@78:         roll-left? (atom false)
rlm@78:         roll-right? (atom false)
rlm@78:         timer (atom 0)]
rlm@78: 
rlm@78:     (world
rlm@83:      (nodify [top bottom floor origin])
rlm@78:      (merge standard-debug-controls
rlm@78:             {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
rlm@78:              "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
rlm@78:              "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@78:              "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
rlm@78:              "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
rlm@78:              "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
rlm@78:      
rlm@78:      (fn [world]
rlm@78:        (light-up-everything world)
rlm@78:        (enable-debug world)
rlm@78:        (set-gravity world (Vector3f. 0 0 0))
rlm@78:        )
rlm@78:      
rlm@78:      (fn [world _]
rlm@78:        (if (zero? (rem (swap! timer inc) 100))
rlm@78:          (do
rlm@78:          ;;  (println-repl @timer)
rlm@78:            (.attachChild (.getRootNode world)
rlm@78:                        (sphere 0.05 :color ColorRGBA/Yellow
rlm@78:                                :position (.getWorldTranslation top)
rlm@83:                                :physical? false))
rlm@83:            (.attachChild (.getRootNode world)
rlm@83:                          (sphere 0.05 :color ColorRGBA/LightGray
rlm@83:                                  :position (.getWorldTranslation bottom)
rlm@83:                                  :physical? false))))
rlm@83: 
rlm@78:        (if @move-up?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. 0 0 10))))
rlm@78:        (if @move-down?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. 0 0 -10))))
rlm@78:        (if @move-left?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. 0 10 0))))
rlm@78:        (if @move-right?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. 0 -10 0))))
rlm@78:        (if @roll-left?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. -1 0 0))))
rlm@78:        (if @roll-right?
rlm@78:          (.applyTorque control
rlm@78:                        (.mult (.getPhysicsRotation control)
rlm@78:                               (Vector3f. 1 0 0))))))))
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: ;; please validate these.
rlm@83: 
rlm@83: (defn get-subjective-position
rlm@83:    "Convert the world coordinates into coordinates relative to
rlm@83:    object (i.e. local coordinates), taking into account the rotation
rlm@83:    of object."
rlm@83:    [#^Vector3f world-coordinates object]
rlm@83:    ;; I don't know if it's important to take into account the rotation
rlm@83:    ;; of the object. If it isn't, then remove the multiplication-by-inverse.
rlm@83:    (.mult (.inverse (.getWorldRotation object))
rlm@83:           (.subtract world-coordinates (.getWorldTranslation object))))
rlm@83:    
rlm@83: 
rlm@83: (defn get-subjective-rotation
rlm@83:   "cf get-subjective-position. Converts a rotation specified relative
rlm@83: to the world's axes into a rotation specified in terms of the object's
rlm@83:   coordinate system."
rlm@83:   [#^Quaternion world-rotation object]
rlm@83:   (.mult (.inverse (.getWorldRotation object)) world-rotation))
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: (defn joint-create "Connect objects 1 and 2 using a joint constraint. If
rlm@83:   only position is specified, creates a point-to-point joint at the
rlm@83:   given location
rlm@83:   in world coordinates. etc. etc. for other joints.
rlm@83: To ensure consistency, I may alter this function
rlm@83:   so that it moves obj-1 to be at the apex of the cone.
rlm@83: 
rlm@83:   NOTE:
rlm@83:   In the usual construction method, you create a joint and, if your
rlm@83:   contraints are consistent, all the objects snap into position and
rlm@83:   orientation around it, otherwise the systen explodes.
rlm@83: 
rlm@83:   This construction method assumes that you have in mind a position and
rlm@83:   rotation for the joint, and that you have already put your objects
rlm@83:   at the required distances from that joint so that no snapping needs
rlm@83:   to occur. The radial distances ('pivot lengths') are therefore just set to be
rlm@83:   the pre-existing distances between the objects and the joint." 
rlm@83:   [#^Node obj-1
rlm@83:    #^Node obj-2
rlm@83:    #^Vector3f joint-position
rlm@83:    #^Quaternion joint-rotation
rlm@83:    span-1
rlm@83:    span-2
rlm@83:    twist
rlm@83:    ]
rlm@83: 
rlm@83:   (let
rlm@83:       [body-1 (.getControl obj-1 RigidBodyControl)
rlm@83:        body-2 (.getControl obj-2 RigidBodyControl)
rlm@83:        ]
rlm@83:     (doto (ConeJoint.
rlm@83:            body-1
rlm@83:            body-2
rlm@83:            (get-subjective-position joint-position body-1)
rlm@83:            (get-subjective-position joint-position body-2)
rlm@83:            ;; ALIGN X-AXIS OF OBJECT-1 WITH THE CENTRAL AXIS OF THE CONE TO
rlm@83:            ;;LOWER THE RISK OF INCONSISTENCY
rlm@83:            ;;(Matrix3f/IDENTITY)
rlm@83:            (.toRotationMatrix (get-subjective-rotation joint-rotation body-1))
rlm@83:            (.toRotationMatrix (get-subjective-rotation joint-rotation body-2))
rlm@83:            )
rlm@83:       (.setLimit
rlm@83:        span-1
rlm@83:        span-2
rlm@83:        twist))))
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@83: 
rlm@78: #+end_src
rlm@78: 
rlm@78: 
rlm@78: * COMMENT purgatory
rlm@78: #+begin_src clojure
rlm@77: (defn bullet-trans []
rlm@77:   (let [obj-a (sphere 0.5 :color ColorRGBA/Red
rlm@77:                       :position (Vector3f. -10 5 0))
rlm@77:         obj-b (sphere 0.5 :color ColorRGBA/Blue
rlm@77:                       :position (Vector3f. -10 -5 0)
rlm@77:                       :mass 0)
rlm@77:         control-a (.getControl obj-a RigidBodyControl)
rlm@77:         control-b (.getControl obj-b RigidBodyControl)
rlm@77:         swivel
rlm@77:         (.toRotationMatrix
rlm@77:          (doto (Quaternion.)
rlm@77:            (.fromAngleAxis (/ Math/PI 2)
rlm@77:                            Vector3f/UNIT_X)))]
rlm@77:     (doto 
rlm@77:         (ConeJoint.
rlm@77:          control-a control-b
rlm@77:          (Vector3f. 0 5 0)
rlm@77:          (Vector3f. 0 -5 0)
rlm@77:          swivel swivel)
rlm@77:       (.setLimit (* 0.6 (/ Math/PI 4))
rlm@77:                  (/ Math/PI 4)
rlm@77:                  (* Math/PI 0.8)))
rlm@77:     (world (nodify
rlm@77:             [obj-a obj-b])
rlm@77:             standard-debug-controls
rlm@77:             enable-debug
rlm@77:             no-op)))
rlm@74: 
rlm@74: 
rlm@77: (defn bullet-trans* []
rlm@77:   (let [obj-a (box 1.5 0.5 0.5 :color ColorRGBA/Red
rlm@77:                    :position (Vector3f. 5 0 0)
rlm@77:                    :mass 90)
rlm@77:         obj-b (sphere 0.5 :color ColorRGBA/Blue
rlm@77:                       :position (Vector3f. -5 0 0)
rlm@77:                       :mass 0)
rlm@77:         control-a (.getControl obj-a RigidBodyControl)
rlm@77:         control-b (.getControl obj-b RigidBodyControl)
rlm@77:         move-up? (atom nil)
rlm@77:         move-down? (atom nil)
rlm@77:         move-left? (atom nil)
rlm@77:         move-right? (atom nil)
rlm@77:         roll-left? (atom nil)
rlm@77:         roll-right? (atom nil)
rlm@77:         force 100
rlm@77:         swivel
rlm@77:         (.toRotationMatrix
rlm@77:          (doto (Quaternion.)
rlm@77:            (.fromAngleAxis (/ Math/PI 2)
rlm@77:                            Vector3f/UNIT_X)))
rlm@77:         x-move
rlm@77:         (doto (Matrix3f.)
rlm@77:           (.fromStartEndVectors Vector3f/UNIT_X
rlm@77:                                 (.normalize (Vector3f. 1 1 0))))
rlm@77:         
rlm@77:         timer (atom 0)]
rlm@77:     (doto 
rlm@77:         (ConeJoint.
rlm@77:          control-a control-b
rlm@77:          (Vector3f. -8 0 0)
rlm@77:          (Vector3f. 2 0 0)
rlm@77:          ;;swivel swivel
rlm@77:          ;;Matrix3f/IDENTITY Matrix3f/IDENTITY
rlm@77:          x-move Matrix3f/IDENTITY 
rlm@77:          )
rlm@77:       (.setCollisionBetweenLinkedBodys false)
rlm@77:       (.setLimit (* 1 (/ Math/PI 4))    ;; twist 
rlm@77:                  (* 1 (/ Math/PI 4))  ;; swing span in X-Y plane
rlm@77:                  (* 0 (/ Math/PI 4))))  ;; swing span in Y-Z plane
rlm@77:     (world (nodify
rlm@77:             [obj-a obj-b])
rlm@77:            (merge standard-debug-controls
rlm@77:                   {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
rlm@77:                    "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
rlm@77:                    "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@77:                    "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
rlm@77:                    "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
rlm@77:                    "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
rlm@77:            
rlm@77:            (fn [world]
rlm@77:              (enable-debug world)
rlm@77:              (set-gravity world Vector3f/ZERO)
rlm@77:              )
rlm@77:                            
rlm@77:            (fn [world _]
rlm@77: 
rlm@77:              (if @move-up?
rlm@77:                (.applyForce control-a
rlm@77:                               (Vector3f. force 0 0)
rlm@77:                               (Vector3f. 0 0 0)))
rlm@77:              (if @move-down?
rlm@77:                (.applyForce control-a
rlm@77:                              (Vector3f. (- force) 0 0)
rlm@77:                              (Vector3f. 0 0 0)))
rlm@77:              (if @move-left?
rlm@77:                (.applyForce control-a
rlm@77:                              (Vector3f. 0 force 0)
rlm@77:                              (Vector3f. 0 0 0)))
rlm@77:              (if @move-right?
rlm@77:                (.applyForce control-a
rlm@77:                              (Vector3f. 0 (- force) 0)
rlm@77:                              (Vector3f. 0 0 0)))
rlm@77: 
rlm@77:              (if @roll-left?
rlm@77:                (.applyForce control-a
rlm@77:                              (Vector3f. 0 0 force)
rlm@77:                              (Vector3f. 0 0 0)))
rlm@77:              (if @roll-right?
rlm@77:                (.applyForce control-a
rlm@77:                              (Vector3f. 0 0 (- force))
rlm@77:                              (Vector3f. 0 0 0)))
rlm@77: 
rlm@77:              (if (zero? (rem (swap! timer inc) 100))
rlm@77:                (.attachChild
rlm@77:                 (.getRootNode world)
rlm@77:                 (sphere 0.05 :color ColorRGBA/Yellow
rlm@77:                         :physical? false :position
rlm@77:                         (.getWorldTranslation obj-a)))))
rlm@77:            )
rlm@77:     ))
rlm@77: 
rlm@77: 
rlm@77: 
rlm@73: #+end_src
rlm@73: 
rlm@73: 
rlm@73: * COMMENT generate source
rlm@73: #+begin_src clojure :tangle ../src/cortex/silly.clj
rlm@73: <<body-1>>
rlm@73: #+end_src
rlm@73: