1.1 --- a/org/body.org	Sat Nov 19 23:59:12 2011 -0700
     1.2 +++ b/org/body.org	Mon Nov 28 02:54:48 2011 -0700
     1.3 @@ -143,6 +143,20 @@
     1.4      node))
     1.5  
     1.6    
     1.7 +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
     1.8 +
     1.9 +;; this could be a good way to give objects special properties like
    1.10 +;; being eyes and the like
    1.11 +
    1.12 +(.getUserData 
    1.13 + (.getChild
    1.14 +  (load-blender-model "Models/property/test.blend") 0)
    1.15 + "properties")
    1.16 +
    1.17 +;; the properties are saved along with the blender file.
    1.18 +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    1.19 +
    1.20 +
    1.21  
    1.22  (defn init-debug-skel-node
    1.23    [f debug-node skeleton]
    1.24 @@ -312,32 +326,27 @@
    1.25    
    1.26  ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    1.27  
    1.28 +;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
    1.29  
    1.30  
    1.31 -
    1.32 -
    1.33 -
    1.34 -;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
    1.35 -(defn joint-control
    1.36 -  [joint]
    1.37 -  (let [physics-space (ref nil)]
    1.38 -    (reify PhysicsControl
    1.39 -      (setPhysicsSpace [this space]
    1.40 -        (dosync
    1.41 -         (ref-set physics-space space))
    1.42 -        (.addJoint space joint))
    1.43 -      (update [this tpf])
    1.44 -      (setSpatial [this spatial])
    1.45 -      (render [this rm vp])
    1.46 -      (getPhysicsSpace [this] (deref physics-space))
    1.47 -      (isEnabled [this] true)
    1.48 -      (setEnabled [this state]))))
    1.49 -
    1.50 +(defrecord JointControl [joint physics-space]
    1.51 +  PhysicsControl
    1.52 +  (setPhysicsSpace [this space]
    1.53 +    (dosync
    1.54 +     (ref-set (:physics-space this) space))
    1.55 +    (.addJoint space (:joint this)))
    1.56 +  (update [this tpf])
    1.57 +  (setSpatial [this spatial])
    1.58 +  (render [this rm vp])
    1.59 +  (getPhysicsSpace [this] (deref (:physics-space this)))
    1.60 +  (isEnabled [this] true)
    1.61 +  (setEnabled [this state]))
    1.62 +  
    1.63  (defn add-joint
    1.64    "Add a joint to a particular object. When the object is added to the
    1.65    PhysicsSpace of a simulation, the joint will also be added"
    1.66    [object joint]
    1.67 -  (let [control (joint-control joint)]
    1.68 +  (let [control (JointControl. joint (ref nil))]
    1.69      (.addControl object control))
    1.70    object)
    1.71  
    1.72 @@ -371,7 +380,7 @@
    1.73  
    1.74  (defn nodify
    1.75    "take a sequence of things that can be attached to a node and return
    1.76 -  a node with all of the attached"
    1.77 +  a node with all of them attached"
    1.78    ([name children]
    1.79       (let [node (Node. name)]
    1.80         (dorun (map #(.attachChild node %) children))
    1.81 @@ -427,19 +436,408 @@
    1.82  ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    1.83  
    1.84  
    1.85 +;;;;;;;;;  Mortor Control  ;;;;;;;;;;;;;
    1.86  
    1.87  
    1.88 +;; surprisingly ehough, terristerial creatures only move by using
    1.89 +;; torque applied to their joints.  There's not a single straight line
    1.90 +;; of force in the human body at all!  (a straight line of force would
    1.91 +;; correspond to some sort of jet or rocket propulseion)
    1.92  
    1.93 +(defn node-seq
    1.94 +  "take a node and return a seq of all its children
    1.95 +   recursively. There will be no nodes left in the resulting
    1.96 +   structure"
    1.97 +  [#^Node node]
    1.98 +  (tree-seq #(isa? (class %) Node) #(.getChildren %) node))
    1.99 +  
   1.100 +  
   1.101 +(defn torque-controls [control]
   1.102 +  (let [torques
   1.103 +        (concat
   1.104 +         (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
   1.105 +              (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
   1.106 +         [Vector3f/UNIT_X])]
   1.107 +    (map (fn [torque-axis]
   1.108 +           (fn [torque]
   1.109 +             (.applyTorque
   1.110 +              control
   1.111 +              (.mult (.mult (.getPhysicsRotation control)
   1.112 +                            torque-axis)
   1.113 +                     (float
   1.114 +                      (* (.getMass control) torque))))))
   1.115 +         torques)))
   1.116  
   1.117 +(defn motor-map
   1.118 +  "Take a creature and generate a function that will enable fine
   1.119 +   grained control over all the creature's limbs."  
   1.120 +  [#^Node creature]
   1.121 +  (let [controls (keep #(.getControl % RigidBodyControl)
   1.122 +                       (node-seq creature))
   1.123 +        limb-controls (reduce concat (map torque-controls controls))
   1.124 +        body-control  (partial map #(%1 %2) limb-controls)]
   1.125 +    body-control))
   1.126  
   1.127  
   1.128  
   1.129 +(defn test-motor-map
   1.130 +  "see how torque works."
   1.131 +  []
   1.132 +  (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
   1.133 +                    :mass 1 :color ColorRGBA/Green)
   1.134 +        motor-map (motor-map finger)]
   1.135 +    (world
   1.136 +     (nodify [finger
   1.137 +              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
   1.138 +                   :color ColorRGBA/Gray)])
   1.139 +     standard-debug-controls
   1.140 +     (fn [world]
   1.141 +       (set-gravity world Vector3f/ZERO)
   1.142 +       (light-up-everything world)
   1.143 +       (.setTimer world (NanoTimer.)))
   1.144 +     (fn [_ _]
   1.145 +       (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
   1.146 +
   1.147 +
   1.148 +
   1.149 +(defn test-torque
   1.150 +  "see how torque works."
   1.151 +  []
   1.152 +  (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
   1.153 +                    :mass 1 :color ColorRGBA/Green)
   1.154 +        move-left? (atom false)
   1.155 +        move-right? (atom false)
   1.156 +        control (.getControl finger RigidBodyControl)]
   1.157 +    (world
   1.158 +     (nodify [finger
   1.159 +              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
   1.160 +                   :color ColorRGBA/Gray)])
   1.161 +     (merge standard-debug-controls
   1.162 +            {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
   1.163 +             "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
   1.164 +     (fn [world]
   1.165 +       (set-gravity world Vector3f/ZERO)
   1.166 +       (light-up-everything world)
   1.167 +       (.setTimer world (NanoTimer.)))
   1.168 +     (fn [_ _]
   1.169 +       (if @move-left?
   1.170 +         (.applyTorque control
   1.171 +                       (.mult (.getPhysicsRotation control)
   1.172 +                       (Vector3f. -3 20 0))))
   1.173 +       (if @move-right?
   1.174 +         (.applyTorque control (Vector3f. 0 0 1)))))))
   1.175 +
   1.176 +
   1.177 +(defn worm-pattern [time]
   1.178 +  [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
   1.179 +
   1.180 +   0 0 0 0 0 0 0 0 0 0 0
   1.181 +
   1.182 +   (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
   1.183 +
   1.184 +   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
   1.185 +  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
   1.186 +  0 0 0 0 0 0 0 0 0 0 0 0 0 0
   1.187 +
   1.188 +   ])
   1.189 +
   1.190 +
   1.191 +;; (defn copier-gen []
   1.192 +;;   (let [count (atom 0)]
   1.193 +;;     (fn [in]
   1.194 +;;       (swap! count inc)
   1.195 +;;       (clojure.contrib.duck-streams/copy
   1.196 +;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"
   1.197 +;;                      ;;/home/r/tmp/mao-test/clojure-images
   1.198 +;;                (format "%08d.png" @count)))))))
   1.199 +;; (defn decrease-framerate []
   1.200 +;;   (map
   1.201 +;;    (copier-gen)
   1.202 +;;    (sort
   1.203 +;;     (map first
   1.204 +;;          (partition
   1.205 +;;           4
   1.206 +;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))
   1.207 +;;                   (file-seq
   1.208 +;;                    (file-str
   1.209 +;;                     "/home/r/media/anime/mao-temp/images"))))))))
   1.210 +
   1.211 +
   1.212 +
   1.213 +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
   1.214 +
   1.215 +
   1.216 +
   1.217 +
   1.218 +;;;;;;;;;;;;;;;;;;  Proprioception  ;;;;;;;;;;;;;;;;;;;;;;;;
   1.219 +
   1.220 +
   1.221 +;; this is not used as just getting the rotation would be simpler.
   1.222 +(defn proprioception-senses 
   1.223 +  "given a control , create a sequence of thunks that will report the
   1.224 +  rotation of the control's object along the same axes as the motor-control map."
   1.225 +  [control]
   1.226 +  (let [torques
   1.227 +        (concat
   1.228 +         (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
   1.229 +              (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
   1.230 +         [Vector3f/UNIT_X])]
   1.231 +    (map (fn [torque-axis]
   1.232 +           (fn []
   1.233 +             (.getPhysicsRotation control)))
   1.234 +           torques)))
   1.235 +
   1.236 +(defn orthogonal-vect
   1.237 +  "Return a vector orthogonal to the current one"
   1.238 +  [vector]
   1.239 +  (let [x (.getX vector)
   1.240 +        y (.getY vector)
   1.241 +        z (.getZ vector)]
   1.242 +    (cond
   1.243 +     (not= x (float 0)) (Vector3f. (- z) 0 x)
   1.244 +     (not= y (float 0)) (Vector3f. 0 (- z) y)
   1.245 +     (not= z (float 0)) (Vector3f. 0 (- z) y)
   1.246 +     true Vector3f/ZERO)))
   1.247 +
   1.248 +;; from
   1.249 +;; http://stackoverflow.com/questions/3684269/ \\
   1.250 +;;   component-of-a-quaternion-rotation-around-an-axis
   1.251 +(defn rot-about-axis [#^Quaternion q #^Vector3f axis]
   1.252 +  (let [basis-1 (orthogonal-vect axis)
   1.253 +        basis-2 (.cross axis basis-1)
   1.254 +        rotated (.mult q basis-1)
   1.255 +        alpha (.dot basis-1 (.project rotated basis-1))
   1.256 +        beta  (.dot basis-2 (.project rotated basis-2))]
   1.257 +    (println-repl alpha)
   1.258 +    (println-repl beta)
   1.259 +    (Math/atan2 beta alpha)))
   1.260 +
   1.261 +
   1.262 +(defn check-rot [a]
   1.263 +  (rot-about-axis
   1.264 +   (doto (Quaternion.)
   1.265 +     (.fromAngleAxis
   1.266 +      (float a)
   1.267 +      (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
   1.268 +
   1.269 +(defn relative-positions [joint]
   1.270 +  (let [object-a (.getUserObject (.getBodyA joint))
   1.271 +        object-b (.getUserObject (.getBodyB joint))
   1.272 +        arm-a
   1.273 +        (.normalize
   1.274 +         (.subtract 
   1.275 +          (.localToWorld object-a (.getPivotA joint) nil)
   1.276 +          (.getWorldTranslation object-a)))
   1.277 +        rotate-a
   1.278 +         (doto (Matrix3f.)
   1.279 +           (.fromStartEndVectors arm-a Vector3f/UNIT_X))
   1.280 +        arm-b
   1.281 +        (.mult
   1.282 +         rotate-a 
   1.283 +         (.normalize
   1.284 +          (.subtract 
   1.285 +           (.localToWorld object-b (.getPivotB joint) nil)
   1.286 +           (.getWorldTranslation object-b))))
   1.287 +        rotate-b
   1.288 +        (doto (Matrix3f.)
   1.289 +          (.fromStartEndVectors arm-b Vector3f/UNIT_X))
   1.290 +        
   1.291 +        pitch
   1.292 +        (.angleBetween
   1.293 +         (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
   1.294 +         (Vector2f. 1 0))
   1.295 +        yaw
   1.296 +        (.angleBetween
   1.297 +         (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
   1.298 +         (Vector2f. 1 0))
   1.299 +                     
   1.300 +        roll
   1.301 +        (.mult 
   1.302 +         (.getLocalRotation object-b)
   1.303 +         (doto (Quaternion.)
   1.304 +           (.fromRotationMatrix rotate-a)))
   1.305 +         ]
   1.306 +         
   1.307 +                
   1.308 +
   1.309 +    ;;(println-repl
   1.310 +    ;; "arm-b is " arm-b)
   1.311 +    ;;(println-repl
   1.312 +    ;; "pivot-b is " (.getPivotB joint))
   1.313 +    ;;(println-repl
   1.314 +    ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
   1.315 +    ;;         pitch yaw roll))
   1.316 +    [pitch yaw roll]))
   1.317 +
   1.318 +        
   1.319 +    
   1.320 +        
   1.321 +  
   1.322 +(defn proprioception
   1.323 +  "Create a proprioception map that reports the rotations of the
   1.324 +  various limbs of the creature's body"
   1.325 +  [creature]
   1.326 +  [#^Node creature]
   1.327 +  (let [
   1.328 +        nodes (node-seq creature)
   1.329 +        joints
   1.330 +        (map
   1.331 +         :joint 
   1.332 +         (filter
   1.333 +          #(isa? (class %) JointControl)
   1.334 +          (reduce
   1.335 +           concat
   1.336 +           (map (fn [node]
   1.337 +                  (map (fn [num] (.getControl node num))
   1.338 +                       (range (.getNumControls node))))
   1.339 +                nodes))))]
   1.340 +    (fn []
   1.341 +      (reduce concat (map relative-positions (list (first joints)))))))
   1.342 +        
   1.343 +
   1.344 +(defn test-worm-control
   1.345 +  []
   1.346 +  (let [worm (point-worm)
   1.347 +        time (atom 0)
   1.348 +        worm-motor-map (motor-map worm)
   1.349 +        body-map (proprioception worm)
   1.350 +        debug-segments
   1.351 +        (map
   1.352 +         #(doto
   1.353 +              (make-shape
   1.354 +               (assoc base-shape
   1.355 +                 :name "debug-line"
   1.356 +                 :physical? false
   1.357 +                 :shape
   1.358 +                 (com.jme3.scene.shape.Line.
   1.359 +                  (.add (.getWorldTranslation %)
   1.360 +                        (Vector3f. -0.2 0 0 ))
   1.361 +                  (.add (.getWorldTranslation %)
   1.362 +                        (Vector3f. 0.2 0 0)))))
   1.363 +                 (.setMaterial (green-x-ray)))
   1.364 +            (drop 1 (node-seq worm)))]
   1.365 +    (world
   1.366 +     (nodify [worm
   1.367 +              (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
   1.368 +                   :color ColorRGBA/Gray)])
   1.369 +     standard-debug-controls
   1.370 +     (fn [world]
   1.371 +       (.attachChild (.getRootNode world) (nodify debug-segments))
   1.372 +       (enable-debug world)
   1.373 +       (light-up-everything world)
   1.374 +       (com.aurellem.capture.Capture/captureVideo
   1.375 +        world
   1.376 +        (file-str "/home/r/proj/cortex/tmp/moving-worm")))
   1.377 +     
   1.378 +     (fn [_ _]
   1.379 +       (dorun
   1.380 +        (map
   1.381 +         (fn [worm-segment
   1.382 +              debug-segment]
   1.383 +           (.rotate
   1.384 +            debug-segment
   1.385 +            (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
   1.386 +         (drop 1 (node-seq worm))
   1.387 +         debug-segments))
   1.388 +       (swap! time inc)
   1.389 +       (println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))
   1.390 +       (Thread/sleep 200)
   1.391 +       (dorun (worm-motor-map
   1.392 +               (worm-pattern @time)))))))
   1.393 +
   1.394 +
   1.395 +
   1.396 +
   1.397 +
   1.398 +(defn test-prop
   1.399 +  "see how torque works."
   1.400 +  []
   1.401 +  (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
   1.402 +                     :mass 0 :color ColorRGBA/Green)
   1.403 +        finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   1.404 +                     :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
   1.405 +        floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
   1.406 +                     :mass 0 :color ColorRGBA/Gray)
   1.407 +
   1.408 +        move-up? (atom false)
   1.409 +        move-down? (atom false)
   1.410 +        move-left? (atom false)
   1.411 +        move-right? (atom false)
   1.412 +        roll-left? (atom false)
   1.413 +        roll-right? (atom false)
   1.414 +        control (.getControl finger RigidBodyControl)
   1.415 +        joint
   1.416 +        (doto 
   1.417 +            (Point2PointJoint.
   1.418 +             (.getControl hand RigidBodyControl)
   1.419 +             control
   1.420 +             (Vector3f. 1.2 0 0)
   1.421 +             (Vector3f. -1.2 0 0 ))
   1.422 +          (.setCollisionBetweenLinkedBodys false))
   1.423 +        time (atom 0)
   1.424 +        ]
   1.425 +    (world
   1.426 +     (nodify [hand finger floor])
   1.427 +     (merge standard-debug-controls
   1.428 +            {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.429 +             "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.430 +             "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
   1.431 +             "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
   1.432 +             "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
   1.433 +             "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     
   1.434 +     (fn [world]
   1.435 +       (set-gravity world Vector3f/ZERO)
   1.436 +       (.setMoveSpeed (.getFlyByCamera world) 50)
   1.437 +       (.setRotationSpeed (.getFlyByCamera world) 50)
   1.438 +       (light-up-everything world)
   1.439 +       (.setTimer world (NanoTimer.))
   1.440 +       )
   1.441 +     (fn [_ _]
   1.442 +       (if @move-up?
   1.443 +         (.applyTorque control
   1.444 +                       (.mult (.getPhysicsRotation control)
   1.445 +                       (Vector3f. 0 0 1))))
   1.446 +       (if @move-down?
   1.447 +         (.applyTorque control
   1.448 +                       (.mult (.getPhysicsRotation control)
   1.449 +                              (Vector3f. 0 0 -1))))
   1.450 +       (if @move-left?
   1.451 +         (.applyTorque control
   1.452 +                       (.mult (.getPhysicsRotation control)
   1.453 +                              (Vector3f. 0 1 0))))
   1.454 +       (if @move-right?
   1.455 +         (.applyTorque control
   1.456 +                       (.mult (.getPhysicsRotation control)
   1.457 +                              (Vector3f. 0 -1 0))))
   1.458 +       (if @roll-left?
   1.459 +         (.applyTorque control
   1.460 +                       (.mult (.getPhysicsRotation control)
   1.461 +                              (Vector3f. -0.1 0 0))))
   1.462 +       (if @roll-right?
   1.463 +         (.applyTorque control
   1.464 +                       (.mult (.getPhysicsRotation control)
   1.465 +                              (Vector3f. 0.1 0 0))))
   1.466 +
   1.467 +       (if (= 0 (rem (swap! time inc) 2000))
   1.468 +         (do 
   1.469 +           ;;(println-repl (.getLocalRotation finger))
   1.470 +           (println-repl (nth (relative-positions joint) 2))))
   1.471 +
   1.472 +       ))))
   1.473 +
   1.474 +
   1.475 +
   1.476 +
   1.477 +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
   1.478  
   1.479  
   1.480  
   1.481  
   1.482  #+end_src
   1.483  
   1.484 +#+results: body-main
   1.485 +: #'cortex.body/test-prop
   1.486 +
   1.487  
   1.488  
   1.489  
   1.490 @@ -452,3 +850,4 @@
   1.491  <<body-main>>
   1.492  #+end_src
   1.493    
   1.494 +