Mercurial > cortex

annotate org/body.org @ 175:0b9ae09eaec3

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
renamed functions in body
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 06:47:07 -0700
parents 33278bf028e7
children 026f69582022
rev   line source
rlm@0 1 #+title: The BODY!!!
rlm@0 2 #+author: Robert McIntyre
rlm@0 3 #+email: rlm@mit.edu
rlm@4 4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
rlm@4 5 #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4 6 #+INCLUDE: ../../aurellem/org/level-0.org
rlm@4 7
rlm@175 8 * Making a solid, connected body.
rlm@175 9 #+name: joints
rlm@0 10 #+begin_src clojure
rlm@44 11 (ns cortex.body
rlm@160 12 (:use (cortex world util sense))
rlm@64 13 (:import
rlm@64 14 com.jme3.math.Vector3f
rlm@64 15 com.jme3.math.Quaternion
rlm@64 16 com.jme3.math.Vector2f
rlm@64 17 com.jme3.math.Matrix3f
rlm@135 18 com.jme3.bullet.control.RigidBodyControl
rlm@135 19 com.jme3.collision.CollisionResults
rlm@150 20 com.jme3.bounding.BoundingBox
rlm@150 21 com.jme3.scene.Node))
rlm@133 22
rlm@175 23 (use 'clojure.contrib.def)
rlm@160 24 (cortex.import/mega-import-jme3)
rlm@160 25
rlm@135 26 (defn joint-targets
rlm@135 27 "Return the two closest two objects to the joint object, ordered
rlm@135 28 from bottom to top according to the joint's rotation."
rlm@135 29 [#^Node parts #^Node joint]
rlm@135 30 (loop [radius (float 0.01)]
rlm@135 31 (let [results (CollisionResults.)]
rlm@135 32 (.collideWith
rlm@135 33 parts
rlm@135 34 (BoundingBox. (.getWorldTranslation joint)
rlm@135 35 radius radius radius)
rlm@135 36 results)
rlm@135 37 (let [targets
rlm@135 38 (distinct
rlm@135 39 (map #(.getGeometry %) results))]
rlm@135 40 (if (>= (count targets) 2)
rlm@135 41 (sort-by
rlm@135 42 #(let [v
rlm@135 43 (jme-to-blender
rlm@135 44 (.mult
rlm@135 45 (.inverse (.getWorldRotation joint))
rlm@135 46 (.subtract (.getWorldTranslation %)
rlm@135 47 (.getWorldTranslation joint))))]
rlm@135 48 (println-repl (.getName %) ":" v)
rlm@135 49 (.dot (Vector3f. 1 1 1)
rlm@135 50 v))
rlm@135 51 (take 2 targets))
rlm@135 52 (recur (float (* radius 2))))))))
rlm@135 53
rlm@160 54 (defmulti joint-dispatch
rlm@160 55 "Translate blender pseudo-joints into real JME joints."
rlm@160 56 (fn [constraints & _]
rlm@160 57 (:type constraints)))
rlm@141 58
rlm@160 59 (defmethod joint-dispatch :point
rlm@160 60 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 61 (println-repl "creating POINT2POINT joint")
rlm@160 62 ;; bullet's point2point joints are BROKEN, so we must use the
rlm@160 63 ;; generic 6DOF joint instead of an actual Point2Point joint!
rlm@141 64
rlm@160 65 ;; should be able to do this:
rlm@160 66 (comment
rlm@160 67 (Point2PointJoint.
rlm@160 68 control-a
rlm@160 69 control-b
rlm@160 70 pivot-a
rlm@160 71 pivot-b))
rlm@141 72
rlm@160 73 ;; but instead we must do this:
rlm@160 74 (println-repl "substuting 6DOF joint for POINT2POINT joint!")
rlm@160 75 (doto
rlm@160 76 (SixDofJoint.
rlm@160 77 control-a
rlm@160 78 control-b
rlm@160 79 pivot-a
rlm@160 80 pivot-b
rlm@160 81 false)
rlm@160 82 (.setLinearLowerLimit Vector3f/ZERO)
rlm@160 83 (.setLinearUpperLimit Vector3f/ZERO)
rlm@160 84 ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
rlm@160 85 ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
rlm@141 86
rlm@160 87 ))
rlm@160 88
rlm@160 89
rlm@160 90 (defmethod joint-dispatch :hinge
rlm@160 91 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 92 (println-repl "creating HINGE joint")
rlm@160 93 (let [axis
rlm@160 94 (if-let
rlm@160 95 [axis (:axis constraints)]
rlm@160 96 axis
rlm@160 97 Vector3f/UNIT_X)
rlm@160 98 [limit-1 limit-2] (:limit constraints)
rlm@160 99 hinge-axis
rlm@160 100 (.mult
rlm@160 101 rotation
rlm@160 102 (blender-to-jme axis))]
rlm@160 103 (doto
rlm@160 104 (HingeJoint.
rlm@160 105 control-a
rlm@160 106 control-b
rlm@160 107 pivot-a
rlm@160 108 pivot-b
rlm@160 109 hinge-axis
rlm@160 110 hinge-axis)
rlm@160 111 (.setLimit limit-1 limit-2))))
rlm@160 112
rlm@160 113 (defmethod joint-dispatch :cone
rlm@160 114 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 115 (let [limit-xz (:limit-xz constraints)
rlm@160 116 limit-xy (:limit-xy constraints)
rlm@160 117 twist (:twist constraints)]
rlm@160 118
rlm@160 119 (println-repl "creating CONE joint")
rlm@160 120 (println-repl rotation)
rlm@160 121 (println-repl
rlm@160 122 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
rlm@160 123 (println-repl
rlm@160 124 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
rlm@160 125 (println-repl
rlm@160 126 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
rlm@160 127 (doto
rlm@160 128 (ConeJoint.
rlm@160 129 control-a
rlm@160 130 control-b
rlm@160 131 pivot-a
rlm@160 132 pivot-b
rlm@160 133 rotation
rlm@160 134 rotation)
rlm@160 135 (.setLimit (float limit-xz)
rlm@160 136 (float limit-xy)
rlm@160 137 (float twist)))))
rlm@160 138
rlm@160 139 (defn connect
rlm@175 140 "Create a joint between 'obj-a and 'obj-b at the location of
rlm@175 141 'joint. The type of joint is determined by the metadata on 'joint.
rlm@175 142
rlm@175 143 Here are some examples:
rlm@160 144 {:type :point}
rlm@160 145 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
rlm@160 146 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
rlm@160 147
rlm@160 148 {:type :cone :limit-xz 0]
rlm@160 149 :limit-xy 0]
rlm@160 150 :twist 0]} (use XZY rotation mode in blender!)"
rlm@160 151 [#^Node obj-a #^Node obj-b #^Node joint]
rlm@160 152 (let [control-a (.getControl obj-a RigidBodyControl)
rlm@160 153 control-b (.getControl obj-b RigidBodyControl)
rlm@160 154 joint-center (.getWorldTranslation joint)
rlm@160 155 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
rlm@160 156 pivot-a (world-to-local obj-a joint-center)
rlm@160 157 pivot-b (world-to-local obj-b joint-center)]
rlm@160 158
rlm@160 159 (if-let [constraints
rlm@160 160 (map-vals
rlm@160 161 eval
rlm@160 162 (read-string
rlm@160 163 (meta-data joint "joint")))]
rlm@160 164 ;; A side-effect of creating a joint registers
rlm@160 165 ;; it with both physics objects which in turn
rlm@160 166 ;; will register the joint with the physics system
rlm@160 167 ;; when the simulation is started.
rlm@160 168 (do
rlm@160 169 (println-repl "creating joint between"
rlm@160 170 (.getName obj-a) "and" (.getName obj-b))
rlm@160 171 (joint-dispatch constraints
rlm@160 172 control-a control-b
rlm@160 173 pivot-a pivot-b
rlm@160 174 joint-rotation))
rlm@160 175 (println-repl "could not find joint meta-data!"))))
rlm@160 176
rlm@175 177 (defvar
rlm@175 178 ^{:arglists '([creature])}
rlm@175 179 joints
rlm@175 180 (sense-nodes "joints")
rlm@175 181 "Return the children of the creature's \"joints\" node.")
rlm@160 182
rlm@175 183 (defn physical!
rlm@175 184 "Iterate through the nodes in creature and make them real physical
rlm@175 185 objects in the simulation."
rlm@175 186 [#^Node creature]
rlm@160 187 (dorun
rlm@160 188 (map
rlm@160 189 (fn [geom]
rlm@160 190 (let [physics-control
rlm@160 191 (RigidBodyControl.
rlm@160 192 (HullCollisionShape.
rlm@160 193 (.getMesh geom))
rlm@160 194 (if-let [mass (meta-data geom "mass")]
rlm@160 195 (do
rlm@160 196 (println-repl
rlm@160 197 "setting" (.getName geom) "mass to" (float mass))
rlm@160 198 (float mass))
rlm@160 199 (float 1)))]
rlm@160 200
rlm@160 201 (.addControl geom physics-control)))
rlm@160 202 (filter #(isa? (class %) Geometry )
rlm@175 203 (node-seq creature)))))
rlm@175 204
rlm@175 205 (defn joints!
rlm@175 206 "Connect the solid parts of the creature with physical joints. The
rlm@175 207 joints are taken from the \"joints\" node in the creature."
rlm@175 208 [#^Node creature]
rlm@160 209 (dorun
rlm@160 210 (map
rlm@160 211 (fn [joint]
rlm@175 212 (let [[obj-a obj-b] (joint-targets creature joint)]
rlm@160 213 (connect obj-a obj-b joint)))
rlm@175 214 (joints creature))))
rlm@160 215
rlm@175 216 (defn body!
rlm@175 217 "Endow the creature with a physical body connected with joints. The
rlm@175 218 particulars of the joints and the masses of each pody part are
rlm@175 219 determined in blender."
rlm@175 220 [#^Node creature]
rlm@175 221 (physical! creature)
rlm@175 222 (joints! creature))
rlm@160 223
rlm@160 224
rlm@160 225
rlm@141 226
rlm@64 227 #+end_src
rlm@63 228
rlm@175 229 #+results: joints
rlm@175 230 : #'cortex.body/body!
rlm@175 231
rlm@133 232 #+results: proprioception
rlm@145 233 : #'cortex.body/proprioception-debug-window
rlm@133 234
rlm@64 235 * Examples
rlm@63 236
rlm@69 237 #+name: test-body
rlm@64 238 #+begin_src clojure
rlm@69 239 (ns cortex.test.body
rlm@64 240 (:use (cortex world util body))
rlm@135 241 (:require cortex.silly)
rlm@64 242 (:import
rlm@64 243 com.jme3.math.Vector3f
rlm@64 244 com.jme3.math.ColorRGBA
rlm@64 245 com.jme3.bullet.joints.Point2PointJoint
rlm@64 246 com.jme3.bullet.control.RigidBodyControl
rlm@145 247 com.jme3.system.NanoTimer
rlm@145 248 com.jme3.math.Quaternion))
rlm@63 249
rlm@64 250 (defn worm-segments
rlm@64 251 "Create multiple evenly spaced box segments. They're fabulous!"
rlm@64 252 [segment-length num-segments interstitial-space radius]
rlm@64 253 (letfn [(nth-segment
rlm@64 254 [n]
rlm@64 255 (box segment-length radius radius :mass 0.1
rlm@64 256 :position
rlm@64 257 (Vector3f.
rlm@64 258 (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@64 259 :name (str "worm-segment" n)
rlm@64 260 :color (ColorRGBA/randomColor)))]
rlm@64 261 (map nth-segment (range num-segments))))
rlm@63 262
rlm@64 263 (defn connect-at-midpoint
rlm@64 264 "Connect two physics objects with a Point2Point joint constraint at
rlm@64 265 the point equidistant from both objects' centers."
rlm@64 266 [segmentA segmentB]
rlm@64 267 (let [centerA (.getWorldTranslation segmentA)
rlm@64 268 centerB (.getWorldTranslation segmentB)
rlm@64 269 midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@64 270 pivotA (.subtract midpoint centerA)
rlm@64 271 pivotB (.subtract midpoint centerB)
rlm@64 272
rlm@64 273 ;; A side-effect of creating a joint registers
rlm@64 274 ;; it with both physics objects which in turn
rlm@64 275 ;; will register the joint with the physics system
rlm@64 276 ;; when the simulation is started.
rlm@64 277 joint (Point2PointJoint.
rlm@64 278 (.getControl segmentA RigidBodyControl)
rlm@64 279 (.getControl segmentB RigidBodyControl)
rlm@64 280 pivotA
rlm@64 281 pivotB)]
rlm@64 282 segmentB))
rlm@63 283
rlm@64 284 (defn eve-worm
rlm@72 285 "Create a worm-like body bound by invisible joint constraints."
rlm@64 286 []
rlm@64 287 (let [segments (worm-segments 0.2 5 0.1 0.1)]
rlm@64 288 (dorun (map (partial apply connect-at-midpoint)
rlm@64 289 (partition 2 1 segments)))
rlm@64 290 (nodify "worm" segments)))
rlm@63 291
rlm@64 292 (defn worm-pattern
rlm@64 293 "This is a simple, mindless motor control pattern that drives the
rlm@64 294 second segment of the worm's body at an offset angle with
rlm@64 295 sinusoidally varying strength."
rlm@64 296 [time]
rlm@64 297 (let [angle (* Math/PI (/ 9 20))
rlm@63 298 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
rlm@63 299 [Vector3f/ZERO
rlm@63 300 (.mult
rlm@63 301 direction
rlm@63 302 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
rlm@63 303 Vector3f/ZERO
rlm@63 304 Vector3f/ZERO
rlm@63 305 Vector3f/ZERO]))
rlm@60 306
rlm@64 307 (defn test-motor-control
rlm@69 308 "Testing motor-control:
rlm@69 309 You should see a multi-segmented worm-like object fall onto the
rlm@64 310 table and begin writhing and moving."
rlm@60 311 []
rlm@64 312 (let [worm (eve-worm)
rlm@60 313 time (atom 0)
rlm@63 314 worm-motor-map (vector-motor-control worm)]
rlm@60 315 (world
rlm@60 316 (nodify [worm
rlm@60 317 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 318 :color ColorRGBA/Gray)])
rlm@60 319 standard-debug-controls
rlm@60 320 (fn [world]
rlm@60 321 (enable-debug world)
rlm@60 322 (light-up-everything world)
rlm@63 323 (comment
rlm@63 324 (com.aurellem.capture.Capture/captureVideo
rlm@63 325 world
rlm@63 326 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@63 327 )
rlm@60 328
rlm@60 329 (fn [_ _]
rlm@60 330 (swap! time inc)
rlm@64 331 (Thread/sleep 20)
rlm@60 332 (dorun (worm-motor-map
rlm@60 333 (worm-pattern @time)))))))
rlm@60 334
rlm@130 335
rlm@135 336
rlm@130 337 (defn join-at-point [obj-a obj-b world-pivot]
rlm@130 338 (cortex.silly/joint-dispatch
rlm@130 339 {:type :point}
rlm@130 340 (.getControl obj-a RigidBodyControl)
rlm@130 341 (.getControl obj-b RigidBodyControl)
rlm@130 342 (cortex.silly/world-to-local obj-a world-pivot)
rlm@130 343 (cortex.silly/world-to-local obj-b world-pivot)
rlm@130 344 nil
rlm@130 345 ))
rlm@130 346
rlm@133 347 (import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@130 348
rlm@130 349 (defn blab-* []
rlm@130 350 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
rlm@130 351 :mass 0 :color ColorRGBA/Green)
rlm@130 352 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
rlm@130 353 :mass 1 :color ColorRGBA/Red)
rlm@130 354 connection-point (Vector3f. 1.2 0 0)
rlm@130 355 root (nodify [hand finger])]
rlm@130 356
rlm@130 357 (join-at-point hand finger (Vector3f. 1.2 0 0))
rlm@130 358
rlm@130 359 (.setCollisionGroup
rlm@130 360 (.getControl hand RigidBodyControl)
rlm@130 361 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130 362 (world
rlm@130 363 root
rlm@130 364 standard-debug-controls
rlm@130 365 (fn [world]
rlm@130 366 (enable-debug world)
rlm@130 367 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@130 368 (set-gravity world Vector3f/ZERO)
rlm@130 369 )
rlm@130 370 no-op)))
rlm@133 371 (comment
rlm@133 372
rlm@133 373 (defn proprioception-debug-window
rlm@133 374 []
rlm@133 375 (let [time (atom 0)]
rlm@133 376 (fn [prop-data]
rlm@133 377 (if (= 0 (rem (swap! time inc) 40))
rlm@133 378 (println-repl prop-data)))))
rlm@133 379 )
rlm@133 380
rlm@131 381 (comment
rlm@131 382 (dorun
rlm@131 383 (map
rlm@131 384 (comp
rlm@131 385 println-repl
rlm@131 386 (fn [[p y r]]
rlm@131 387 (format
rlm@131 388 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@131 389 p y r)))
rlm@131 390 prop-data)))
rlm@131 391
rlm@130 392
rlm@130 393
rlm@137 394
rlm@64 395 (defn test-proprioception
rlm@69 396 "Testing proprioception:
rlm@69 397 You should see two foating bars, and a printout of pitch, yaw, and
rlm@64 398 roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@64 399 change only the value of pitch. key-f/key-g moves it side to side
rlm@64 400 and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@64 401 and counterclockwise, and only affect roll."
rlm@60 402 []
rlm@145 403 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@142 404 :mass 0 :color ColorRGBA/Green :name "hand")
rlm@145 405 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@132 406 :mass 1 :color ColorRGBA/Red :name "finger")
rlm@133 407 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@145 408 :position (Vector3f. 0 1.2 0)
rlm@145 409 :rotation (doto (Quaternion.)
rlm@145 410 (.fromAngleAxis
rlm@145 411 (/ Math/PI 2)
rlm@145 412 (Vector3f. 0 0 1)))
rlm@133 413 :physical? false)
rlm@145 414 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@135 415 creature (nodify [hand finger joint-node])
rlm@145 416 finger-control (.getControl finger RigidBodyControl)
rlm@145 417 hand-control (.getControl hand RigidBodyControl)]
rlm@145 418
rlm@145 419
rlm@145 420 (let
rlm@135 421 ;; *******************************************
rlm@137 422
rlm@145 423 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@135 424 :mass 0 :color ColorRGBA/Gray)
rlm@137 425
rlm@137 426 root (nodify [creature floor])
rlm@133 427 prop (joint-proprioception creature joint-node)
rlm@139 428 prop-view (proprioception-debug-window)
rlm@139 429
rlm@139 430 controls
rlm@139 431 (merge standard-debug-controls
rlm@140 432 {"key-o"
rlm@139 433 (fn [_ _] (.setEnabled finger-control true))
rlm@140 434 "key-p"
rlm@139 435 (fn [_ _] (.setEnabled finger-control false))
rlm@140 436 "key-k"
rlm@140 437 (fn [_ _] (.setEnabled hand-control true))
rlm@140 438 "key-l"
rlm@140 439 (fn [_ _] (.setEnabled hand-control false))
rlm@139 440 "key-i"
rlm@139 441 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@142 442 "key-period"
rlm@142 443 (fn [world _]
rlm@142 444 (.setEnabled finger-control false)
rlm@142 445 (.setEnabled hand-control false)
rlm@142 446 (.rotate creature (doto (Quaternion.)
rlm@142 447 (.fromAngleAxis
rlm@142 448 (float (/ Math/PI 15))
rlm@142 449 (Vector3f. 0 0 -1))))
rlm@142 450
rlm@142 451 (.setEnabled finger-control true)
rlm@142 452 (.setEnabled hand-control true)
rlm@142 453 (set-gravity world (Vector3f. 0 0 0))
rlm@142 454 )
rlm@142 455
rlm@142 456
rlm@139 457 }
rlm@139 458 )
rlm@130 459
rlm@139 460 ]
rlm@139 461 (comment
rlm@139 462 (.setCollisionGroup
rlm@139 463 (.getControl hand RigidBodyControl)
rlm@139 464 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@139 465 )
rlm@140 466 (apply
rlm@140 467 world
rlm@140 468 (with-movement
rlm@140 469 hand
rlm@140 470 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@140 471 [10 10 10 10 1 1]
rlm@140 472 (with-movement
rlm@140 473 finger
rlm@140 474 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@145 475 [1 1 10 10 10 10]
rlm@140 476 [root
rlm@140 477 controls
rlm@140 478 (fn [world]
rlm@140 479 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@140 480 (set-gravity world (Vector3f. 0 0 0))
rlm@140 481 (light-up-everything world))
rlm@145 482 (fn [_ _] (prop-view (list (prop))))]))))))
rlm@138 483
rlm@64 484 #+end_src
rlm@56 485
rlm@130 486 #+results: test-body
rlm@130 487 : #'cortex.test.body/test-proprioception
rlm@130 488
rlm@60 489
rlm@63 490 * COMMENT code-limbo
rlm@61 491 #+begin_src clojure
rlm@61 492 ;;(.loadModel
rlm@61 493 ;; (doto (asset-manager)
rlm@61 494 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 495 ;; "Models/person/person.blend")
rlm@61 496
rlm@64 497
rlm@64 498 (defn load-blender-model
rlm@64 499 "Load a .blend file using an asset folder relative path."
rlm@64 500 [^String model]
rlm@64 501 (.loadModel
rlm@64 502 (doto (asset-manager)
rlm@64 503 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@64 504 model))
rlm@64 505
rlm@64 506
rlm@61 507 (defn view-model [^String model]
rlm@61 508 (view
rlm@61 509 (.loadModel
rlm@61 510 (doto (asset-manager)
rlm@61 511 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 512 model)))
rlm@61 513
rlm@61 514 (defn load-blender-scene [^String model]
rlm@61 515 (.loadModel
rlm@61 516 (doto (asset-manager)
rlm@61 517 (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@61 518 model))
rlm@61 519
rlm@61 520 (defn worm
rlm@61 521 []
rlm@61 522 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@61 523
rlm@61 524 (defn oto
rlm@61 525 []
rlm@61 526 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@61 527
rlm@61 528 (defn sinbad
rlm@61 529 []
rlm@61 530 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@61 531
rlm@61 532 (defn worm-blender
rlm@61 533 []
rlm@61 534 (first (seq (.getChildren (load-blender-model
rlm@61 535 "Models/anim2/simple-worm.blend")))))
rlm@61 536
rlm@61 537 (defn body
rlm@61 538 "given a node with a SkeletonControl, will produce a body sutiable
rlm@61 539 for AI control with movement and proprioception."
rlm@61 540 [node]
rlm@61 541 (let [skeleton-control (.getControl node SkeletonControl)
rlm@61 542 krc (KinematicRagdollControl.)]
rlm@61 543 (comment
rlm@61 544 (dorun
rlm@61 545 (map #(.addBoneName krc %)
rlm@61 546 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@61 547 ;;"mid2" "mid3" "tail" "head"]
rlm@61 548 )))
rlm@61 549 (.addControl node krc)
rlm@61 550 (.setRagdollMode krc)
rlm@61 551 )
rlm@61 552 node
rlm@61 553 )
rlm@61 554 (defn show-skeleton [node]
rlm@61 555 (let [sd
rlm@61 556
rlm@61 557 (doto
rlm@61 558 (SkeletonDebugger. "aurellem-skel-debug"
rlm@61 559 (skel node))
rlm@61 560 (.setMaterial (green-x-ray)))]
rlm@61 561 (.attachChild node sd)
rlm@61 562 node))
rlm@61 563
rlm@61 564
rlm@61 565
rlm@61 566 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 567
rlm@61 568 ;; this could be a good way to give objects special properties like
rlm@61 569 ;; being eyes and the like
rlm@61 570
rlm@61 571 (.getUserData
rlm@61 572 (.getChild
rlm@61 573 (load-blender-model "Models/property/test.blend") 0)
rlm@61 574 "properties")
rlm@61 575
rlm@61 576 ;; the properties are saved along with the blender file.
rlm@61 577 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 578
rlm@61 579
rlm@61 580
rlm@61 581
rlm@61 582 (defn init-debug-skel-node
rlm@61 583 [f debug-node skeleton]
rlm@61 584 (let [bones
rlm@61 585 (map #(.getBone skeleton %)
rlm@61 586 (range (.getBoneCount skeleton)))]
rlm@61 587 (dorun (map #(.setUserControl % true) bones))
rlm@61 588 (dorun (map (fn [b]
rlm@61 589 (println (.getName b)
rlm@61 590 " -- " (f b)))
rlm@61 591 bones))
rlm@61 592 (dorun
rlm@61 593 (map #(.attachChild
rlm@61 594 debug-node
rlm@61 595 (doto
rlm@61 596 (sphere 0.1
rlm@61 597 :position (f %)
rlm@61 598 :physical? false)
rlm@61 599 (.setMaterial (green-x-ray))))
rlm@61 600 bones)))
rlm@61 601 debug-node)
rlm@61 602
rlm@61 603 (import jme3test.bullet.PhysicsTestHelper)
rlm@61 604
rlm@61 605
rlm@61 606 (defn test-zzz [the-worm world value]
rlm@61 607 (if (not value)
rlm@61 608 (let [skeleton (skel the-worm)]
rlm@61 609 (println-repl "enabling bones")
rlm@61 610 (dorun
rlm@61 611 (map
rlm@61 612 #(.setUserControl (.getBone skeleton %) true)
rlm@61 613 (range (.getBoneCount skeleton))))
rlm@61 614
rlm@61 615
rlm@61 616 (let [b (.getBone skeleton 2)]
rlm@61 617 (println-repl "moving " (.getName b))
rlm@61 618 (println-repl (.getLocalPosition b))
rlm@61 619 (.setUserTransforms b
rlm@61 620 Vector3f/UNIT_X
rlm@61 621 Quaternion/IDENTITY
rlm@61 622 ;;(doto (Quaternion.)
rlm@61 623 ;; (.fromAngles (/ Math/PI 2)
rlm@61 624 ;; 0
rlm@61 625 ;; 0
rlm@61 626
rlm@61 627 (Vector3f. 1 1 1))
rlm@61 628 )
rlm@61 629
rlm@61 630 (println-repl "hi! <3"))))
rlm@61 631
rlm@61 632
rlm@61 633 (defn test-ragdoll []
rlm@61 634
rlm@61 635 (let [the-worm
rlm@61 636
rlm@61 637 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@61 638 (doto (show-skeleton (worm-blender))
rlm@61 639 (.setLocalTranslation (Vector3f. 0 10 0))
rlm@61 640 ;;(worm)
rlm@61 641 ;;(oto)
rlm@61 642 ;;(sinbad)
rlm@61 643 )
rlm@61 644 ]
rlm@61 645
rlm@61 646
rlm@61 647 (.start
rlm@61 648 (world
rlm@61 649 (doto (Node.)
rlm@61 650 (.attachChild the-worm))
rlm@61 651 {"key-return" (fire-cannon-ball)
rlm@61 652 "key-space" (partial test-zzz the-worm)
rlm@61 653 }
rlm@61 654 (fn [world]
rlm@61 655 (light-up-everything world)
rlm@61 656 (PhysicsTestHelper/createPhysicsTestWorld
rlm@61 657 (.getRootNode world)
rlm@61 658 (asset-manager)
rlm@61 659 (.getPhysicsSpace
rlm@61 660 (.getState (.getStateManager world) BulletAppState)))
rlm@61 661 (set-gravity world Vector3f/ZERO)
rlm@61 662 ;;(.setTimer world (NanoTimer.))
rlm@61 663 ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@61 664 )
rlm@61 665 no-op
rlm@61 666 )
rlm@61 667
rlm@61 668
rlm@61 669 )))
rlm@61 670
rlm@61 671
rlm@61 672 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 673 ;;; here is the ragdoll stuff
rlm@61 674
rlm@61 675 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@61 676 (def mesh worm-mesh)
rlm@61 677
rlm@61 678 (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@61 679 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 680 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 681
rlm@61 682
rlm@61 683 (defn position [index]
rlm@61 684 (.get
rlm@61 685 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@61 686 index))
rlm@61 687
rlm@61 688 (defn bones [index]
rlm@61 689 (.get
rlm@61 690 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 691 index))
rlm@61 692
rlm@61 693 (defn bone-weights [index]
rlm@61 694 (.get
rlm@61 695 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 696 index))
rlm@61 697
rlm@61 698
rlm@61 699
rlm@61 700 (defn vertex-bones [vertex]
rlm@61 701 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 702
rlm@61 703 (defn vertex-weights [vertex]
rlm@61 704 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 705
rlm@61 706 (defn vertex-position [index]
rlm@61 707 (let [offset (* index 3)]
rlm@61 708 (Vector3f. (position offset)
rlm@61 709 (position (inc offset))
rlm@61 710 (position (inc(inc offset))))))
rlm@61 711
rlm@61 712 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@61 713
rlm@61 714 (defn bone-control-color [index]
rlm@61 715 (get {[1 0 0 0] ColorRGBA/Red
rlm@61 716 [1 2 0 0] ColorRGBA/Magenta
rlm@61 717 [2 0 0 0] ColorRGBA/Blue}
rlm@61 718 (vertex-bones index)
rlm@61 719 ColorRGBA/White))
rlm@61 720
rlm@61 721 (defn influence-color [index bone-num]
rlm@61 722 (get
rlm@61 723 {(float 0) ColorRGBA/Blue
rlm@61 724 (float 0.5) ColorRGBA/Green
rlm@61 725 (float 1) ColorRGBA/Red}
rlm@61 726 ;; find the weight of the desired bone
rlm@61 727 ((zipmap (vertex-bones index)(vertex-weights index))
rlm@61 728 bone-num)
rlm@61 729 ColorRGBA/Blue))
rlm@61 730
rlm@61 731 (def worm-vertices (set (map vertex-info (range 60))))
rlm@61 732
rlm@61 733
rlm@61 734 (defn test-info []
rlm@61 735 (let [points (Node.)]
rlm@61 736 (dorun
rlm@61 737 (map #(.attachChild points %)
rlm@61 738 (map #(sphere 0.01
rlm@61 739 :position (vertex-position %)
rlm@61 740 :color (influence-color % 1)
rlm@61 741 :physical? false)
rlm@61 742 (range 60))))
rlm@61 743 (view points)))
rlm@61 744
rlm@61 745
rlm@61 746 (defrecord JointControl [joint physics-space]
rlm@61 747 PhysicsControl
rlm@61 748 (setPhysicsSpace [this space]
rlm@61 749 (dosync
rlm@61 750 (ref-set (:physics-space this) space))
rlm@61 751 (.addJoint space (:joint this)))
rlm@61 752 (update [this tpf])
rlm@61 753 (setSpatial [this spatial])
rlm@61 754 (render [this rm vp])
rlm@61 755 (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@61 756 (isEnabled [this] true)
rlm@61 757 (setEnabled [this state]))
rlm@61 758
rlm@61 759 (defn add-joint
rlm@61 760 "Add a joint to a particular object. When the object is added to the
rlm@61 761 PhysicsSpace of a simulation, the joint will also be added"
rlm@61 762 [object joint]
rlm@61 763 (let [control (JointControl. joint (ref nil))]
rlm@61 764 (.addControl object control))
rlm@61 765 object)
rlm@61 766
rlm@61 767
rlm@61 768 (defn hinge-world
rlm@61 769 []
rlm@61 770 (let [sphere1 (sphere)
rlm@61 771 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@61 772 joint (Point2PointJoint.
rlm@61 773 (.getControl sphere1 RigidBodyControl)
rlm@61 774 (.getControl sphere2 RigidBodyControl)
rlm@61 775 Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@61 776 (add-joint sphere1 joint)
rlm@61 777 (doto (Node. "hinge-world")
rlm@61 778 (.attachChild sphere1)
rlm@61 779 (.attachChild sphere2))))
rlm@61 780
rlm@61 781
rlm@61 782 (defn test-joint []
rlm@61 783 (view (hinge-world)))
rlm@61 784
rlm@61 785 ;; (defn copier-gen []
rlm@61 786 ;; (let [count (atom 0)]
rlm@61 787 ;; (fn [in]
rlm@61 788 ;; (swap! count inc)
rlm@61 789 ;; (clojure.contrib.duck-streams/copy
rlm@61 790 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@61 791 ;; ;;/home/r/tmp/mao-test/clojure-images
rlm@61 792 ;; (format "%08d.png" @count)))))))
rlm@61 793 ;; (defn decrease-framerate []
rlm@61 794 ;; (map
rlm@61 795 ;; (copier-gen)
rlm@61 796 ;; (sort
rlm@61 797 ;; (map first
rlm@61 798 ;; (partition
rlm@61 799 ;; 4
rlm@61 800 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@61 801 ;; (file-seq
rlm@61 802 ;; (file-str
rlm@61 803 ;; "/home/r/media/anime/mao-temp/images"))))))))
rlm@61 804
rlm@61 805
rlm@61 806
rlm@61 807 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 808
rlm@61 809 (defn proprioception
rlm@61 810 "Create a proprioception map that reports the rotations of the
rlm@61 811 various limbs of the creature's body"
rlm@61 812 [creature]
rlm@61 813 [#^Node creature]
rlm@61 814 (let [
rlm@61 815 nodes (node-seq creature)
rlm@61 816 joints
rlm@61 817 (map
rlm@61 818 :joint
rlm@61 819 (filter
rlm@61 820 #(isa? (class %) JointControl)
rlm@61 821 (reduce
rlm@61 822 concat
rlm@61 823 (map (fn [node]
rlm@61 824 (map (fn [num] (.getControl node num))
rlm@61 825 (range (.getNumControls node))))
rlm@61 826 nodes))))]
rlm@61 827 (fn []
rlm@61 828 (reduce concat (map relative-positions (list (first joints)))))))
rlm@61 829
rlm@61 830
rlm@63 831 (defn skel [node]
rlm@63 832 (doto
rlm@63 833 (.getSkeleton
rlm@63 834 (.getControl node SkeletonControl))
rlm@63 835 ;; this is necessary to force the skeleton to have accurate world
rlm@63 836 ;; transforms before it is rendered to the screen.
rlm@63 837 (.resetAndUpdate)))
rlm@63 838
rlm@63 839 (defn green-x-ray []
rlm@63 840 (doto (Material. (asset-manager)
rlm@63 841 "Common/MatDefs/Misc/Unshaded.j3md")
rlm@63 842 (.setColor "Color" ColorRGBA/Green)
rlm@63 843 (-> (.getAdditionalRenderState)
rlm@63 844 (.setDepthTest false))))
rlm@63 845
rlm@63 846 (defn test-worm []
rlm@63 847 (.start
rlm@63 848 (world
rlm@63 849 (doto (Node.)
rlm@63 850 ;;(.attachChild (point-worm))
rlm@63 851 (.attachChild (load-blender-model
rlm@63 852 "Models/anim2/joint-worm.blend"))
rlm@63 853
rlm@63 854 (.attachChild (box 10 1 10
rlm@63 855 :position (Vector3f. 0 -2 0) :mass 0
rlm@63 856 :color (ColorRGBA/Gray))))
rlm@63 857 {
rlm@63 858 "key-space" (fire-cannon-ball)
rlm@63 859 }
rlm@63 860 (fn [world]
rlm@63 861 (enable-debug world)
rlm@63 862 (light-up-everything world)
rlm@63 863 ;;(.setTimer world (NanoTimer.))
rlm@63 864 )
rlm@63 865 no-op)))
rlm@63 866
rlm@63 867
rlm@63 868
rlm@63 869 ;; defunct movement stuff
rlm@63 870 (defn torque-controls [control]
rlm@63 871 (let [torques
rlm@63 872 (concat
rlm@63 873 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@63 874 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@63 875 [Vector3f/UNIT_X])]
rlm@63 876 (map (fn [torque-axis]
rlm@63 877 (fn [torque]
rlm@63 878 (.applyTorque
rlm@63 879 control
rlm@63 880 (.mult (.mult (.getPhysicsRotation control)
rlm@63 881 torque-axis)
rlm@63 882 (float
rlm@63 883 (* (.getMass control) torque))))))
rlm@63 884 torques)))
rlm@63 885
rlm@63 886 (defn motor-map
rlm@63 887 "Take a creature and generate a function that will enable fine
rlm@63 888 grained control over all the creature's limbs."
rlm@63 889 [#^Node creature]
rlm@63 890 (let [controls (keep #(.getControl % RigidBodyControl)
rlm@63 891 (node-seq creature))
rlm@63 892 limb-controls (reduce concat (map torque-controls controls))
rlm@63 893 body-control (partial map #(%1 %2) limb-controls)]
rlm@63 894 body-control))
rlm@63 895
rlm@63 896 (defn test-motor-map
rlm@63 897 "see how torque works."
rlm@63 898 []
rlm@63 899 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@63 900 :mass 1 :color ColorRGBA/Green)
rlm@63 901 motor-map (motor-map finger)]
rlm@63 902 (world
rlm@63 903 (nodify [finger
rlm@63 904 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@63 905 :color ColorRGBA/Gray)])
rlm@63 906 standard-debug-controls
rlm@63 907 (fn [world]
rlm@63 908 (set-gravity world Vector3f/ZERO)
rlm@63 909 (light-up-everything world)
rlm@63 910 (.setTimer world (NanoTimer.)))
rlm@63 911 (fn [_ _]
rlm@145 912 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
rlm@145 913 0]))))))
rlm@145 914
rlm@145 915 (defn joint-proprioception [#^Node parts #^Node joint]
rlm@145 916 (let [[obj-a obj-b] (joint-targets parts joint)
rlm@145 917 joint-rot (.getWorldRotation joint)
rlm@145 918 pre-inv-a (.inverse (.getWorldRotation obj-a))
rlm@145 919 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
rlm@145 920 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
rlm@145 921 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
rlm@145 922
rlm@145 923 x Vector3f/UNIT_Y
rlm@145 924 y Vector3f/UNIT_Z
rlm@145 925 z Vector3f/UNIT_X
rlm@145 926
rlm@145 927
rlm@145 928 tmp-rot-a (.getWorldRotation obj-a)]
rlm@145 929 (println-repl "x:" (.mult tmp-rot-a x))
rlm@145 930 (println-repl "y:" (.mult tmp-rot-a y))
rlm@145 931 (println-repl "z:" (.mult tmp-rot-a z))
rlm@145 932 (println-repl "rot-a" (.getWorldRotation obj-a))
rlm@145 933 (println-repl "rot-b" (.getWorldRotation obj-b))
rlm@145 934 (println-repl "joint-rot" joint-rot)
rlm@145 935 ;; this function will report proprioceptive information for the
rlm@145 936 ;; joint.
rlm@145 937 (fn []
rlm@145 938 ;; x is the "twist" axis, y and z are the "bend" axes
rlm@145 939 (let [rot-a (.getWorldRotation obj-a)
rlm@145 940 ;;inv-a (.inverse rot-a)
rlm@145 941 rot-b (.getWorldRotation obj-b)
rlm@145 942 ;;relative (.mult rot-b inv-a)
rlm@145 943 basis (doto (Matrix3f.)
rlm@145 944 (.setColumn 0 (.mult rot-a x))
rlm@145 945 (.setColumn 1 (.mult rot-a y))
rlm@145 946 (.setColumn 2 (.mult rot-a z)))
rlm@145 947 rotation-about-joint
rlm@145 948 (doto (Quaternion.)
rlm@145 949 (.fromRotationMatrix
rlm@145 950 (.mult (.invert basis)
rlm@145 951 (.toRotationMatrix rot-b))))
rlm@145 952 [yaw roll pitch]
rlm@145 953 (seq (.toAngles rotation-about-joint nil))]
rlm@145 954 ;;return euler angles of the quaternion around the new basis
rlm@145 955 [yaw roll pitch]))))
rlm@145 956
rlm@61 957 #+end_src
rlm@0 958
rlm@0 959
rlm@0 960
rlm@0 961
rlm@0 962
rlm@0 963
rlm@0 964
rlm@73 965 * COMMENT generate Source
rlm@44 966 #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@175 967 <<joints>>
rlm@0 968 #+end_src
rlm@64 969
rlm@69 970 #+begin_src clojure :tangle ../src/cortex/test/body.clj
rlm@64 971 <<test-body>>
rlm@64 972 #+end_src
rlm@64 973
rlm@64 974
rlm@0 975