Mercurial > cortex

annotate org/body.org @ 60:e5e627f50a3a

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
finally got euler angle stuff working
author Robert McIntyre <rlm@mit.edu>
date Mon, 28 Nov 2011 02:54:48 -0700
parents 25142dad240a
children 7b44348af538
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@58 8 * Body
rlm@0 9
rlm@0 10 #+srcname: body-main
rlm@0 11 #+begin_src clojure
rlm@44 12 (ns cortex.body
rlm@44 13 (use (cortex world util import)))
rlm@44 14
rlm@0 15 (use 'clojure.contrib.def)
rlm@0 16 (cortex.import/mega-import-jme3)
rlm@0 17 (rlm.rlm-commands/help)
rlm@0 18
rlm@44 19 ;;(.loadModel
rlm@44 20 ;; (doto (asset-manager)
rlm@44 21 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@44 22 ;; "Models/person/person.blend")
rlm@44 23
rlm@44 24 (defn view-model [^String model]
rlm@44 25 (view
rlm@44 26 (.loadModel
rlm@44 27 (doto (asset-manager)
rlm@44 28 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@44 29 model)))
rlm@49 30
rlm@49 31 (defn load-blender-scene [^String model]
rlm@49 32 (.loadModel
rlm@49 33 (doto (asset-manager)
rlm@49 34 (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@49 35 model))
rlm@49 36
rlm@49 37 (defn load-blender-model
rlm@49 38 [^String model]
rlm@49 39 (.loadModel
rlm@49 40 (doto (asset-manager)
rlm@49 41 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@49 42 model))
rlm@49 43
rlm@49 44 (defn worm
rlm@49 45 []
rlm@50 46 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@49 47
rlm@50 48 (defn oto
rlm@49 49 []
rlm@49 50 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@49 51
rlm@50 52 (defn sinbad
rlm@50 53 []
rlm@50 54 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@50 55
rlm@50 56 (defn worm-blender
rlm@50 57 []
rlm@50 58 (first (seq (.getChildren (load-blender-model
rlm@56 59 "Models/anim2/simple-worm.blend")))))
rlm@50 60
rlm@52 61 (defn skel [node]
rlm@52 62 (doto
rlm@52 63 (.getSkeleton
rlm@52 64 (.getControl node SkeletonControl))
rlm@52 65 ;; this is necessary to force the skeleton to have accurate world
rlm@52 66 ;; transforms before it is rendered to the screen.
rlm@52 67 (.resetAndUpdate)))
rlm@52 68
rlm@50 69 (defprotocol Textual
rlm@50 70 (text [something]
rlm@50 71 "Display a detailed textual analysis of the given object."))
rlm@50 72
rlm@50 73 (extend-type com.jme3.scene.Node
rlm@50 74 Textual
rlm@50 75 (text [node]
rlm@50 76 (println "Total Vertexes: " (.getVertexCount node))
rlm@50 77 (println "Total Triangles: " (.getTriangleCount node))
rlm@50 78 (println "Controls :")
rlm@50 79 (dorun (map #(text (.getControl node %)) (range (.getNumControls node))))
rlm@50 80 (println "Has " (.getQuantity node) " Children:")
rlm@50 81 (doall (map text (.getChildren node)))))
rlm@50 82
rlm@50 83 (extend-type com.jme3.animation.AnimControl
rlm@50 84 Textual
rlm@50 85 (text [control]
rlm@50 86 (let [animations (.getAnimationNames control)]
rlm@52 87 (println "Animation Control with " (count animations) " animation(s):")
rlm@50 88 (dorun (map println animations)))))
rlm@50 89
rlm@50 90 (extend-type com.jme3.animation.SkeletonControl
rlm@50 91 Textual
rlm@50 92 (text [control]
rlm@50 93 (println "Skeleton Control with the following skeleton:")
rlm@50 94 (println (.getSkeleton control))))
rlm@50 95
rlm@52 96 (extend-type com.jme3.bullet.control.KinematicRagdollControl
rlm@52 97 Textual
rlm@52 98 (text [control]
rlm@52 99 (println "Ragdoll Control")))
rlm@52 100
rlm@52 101
rlm@50 102 (extend-type com.jme3.scene.Geometry
rlm@50 103 Textual
rlm@50 104 (text [control]
rlm@50 105 (println "...geo...")))
rlm@50 106
rlm@50 107
rlm@50 108
rlm@50 109
rlm@50 110 (defn body
rlm@52 111 "given a node with a SkeletonControl, will produce a body sutiable
rlm@52 112 for AI control with movement and proprioception."
rlm@50 113 [node]
rlm@50 114 (let [skeleton-control (.getControl node SkeletonControl)
rlm@53 115 krc (KinematicRagdollControl.)]
rlm@53 116 (comment
rlm@50 117 (dorun
rlm@50 118 (map #(.addBoneName krc %)
rlm@53 119 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@53 120 ;;"mid2" "mid3" "tail" "head"]
rlm@53 121 )))
rlm@50 122 (.addControl node krc)
rlm@52 123 (.setRagdollMode krc)
rlm@52 124 )
rlm@50 125 node
rlm@50 126 )
rlm@50 127
rlm@52 128 (defn green-x-ray []
rlm@51 129 (doto (Material. (asset-manager)
rlm@51 130 "Common/MatDefs/Misc/Unshaded.j3md")
rlm@51 131 (.setColor "Color" ColorRGBA/Green)
rlm@52 132 (-> (.getAdditionalRenderState)
rlm@52 133 (.setDepthTest false))))
rlm@51 134
rlm@53 135 (defn show-skeleton [node]
rlm@50 136 (let [sd
rlm@51 137
rlm@50 138 (doto
rlm@51 139 (SkeletonDebugger. "aurellem-skel-debug"
rlm@51 140 (skel node))
rlm@51 141 (.setMaterial (green-x-ray)))]
rlm@53 142 (.attachChild node sd)
rlm@53 143 node))
rlm@50 144
rlm@51 145
rlm@60 146 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60 147
rlm@60 148 ;; this could be a good way to give objects special properties like
rlm@60 149 ;; being eyes and the like
rlm@60 150
rlm@60 151 (.getUserData
rlm@60 152 (.getChild
rlm@60 153 (load-blender-model "Models/property/test.blend") 0)
rlm@60 154 "properties")
rlm@60 155
rlm@60 156 ;; the properties are saved along with the blender file.
rlm@60 157 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60 158
rlm@60 159
rlm@50 160
rlm@50 161 (defn init-debug-skel-node
rlm@50 162 [f debug-node skeleton]
rlm@50 163 (let [bones
rlm@50 164 (map #(.getBone skeleton %)
rlm@50 165 (range (.getBoneCount skeleton)))]
rlm@50 166 (dorun (map #(.setUserControl % true) bones))
rlm@50 167 (dorun (map (fn [b]
rlm@50 168 (println (.getName b)
rlm@50 169 " -- " (f b)))
rlm@50 170 bones))
rlm@50 171 (dorun
rlm@50 172 (map #(.attachChild
rlm@50 173 debug-node
rlm@51 174 (doto
rlm@51 175 (sphere 0.1
rlm@51 176 :position (f %)
rlm@51 177 :physical? false)
rlm@51 178 (.setMaterial (green-x-ray))))
rlm@50 179 bones)))
rlm@50 180 debug-node)
rlm@50 181
rlm@52 182 (import jme3test.bullet.PhysicsTestHelper)
rlm@50 183
rlm@53 184
rlm@53 185 (defn test-zzz [the-worm world value]
rlm@53 186 (if (not value)
rlm@53 187 (let [skeleton (skel the-worm)]
rlm@53 188 (println-repl "enabling bones")
rlm@53 189 (dorun
rlm@53 190 (map
rlm@53 191 #(.setUserControl (.getBone skeleton %) true)
rlm@53 192 (range (.getBoneCount skeleton))))
rlm@53 193
rlm@53 194
rlm@56 195 (let [b (.getBone skeleton 2)]
rlm@53 196 (println-repl "moving " (.getName b))
rlm@53 197 (println-repl (.getLocalPosition b))
rlm@53 198 (.setUserTransforms b
rlm@54 199 Vector3f/UNIT_X
rlm@53 200 Quaternion/IDENTITY
rlm@54 201 ;;(doto (Quaternion.)
rlm@54 202 ;; (.fromAngles (/ Math/PI 2)
rlm@54 203 ;; 0
rlm@54 204 ;; 0
rlm@54 205
rlm@53 206 (Vector3f. 1 1 1))
rlm@53 207 )
rlm@53 208
rlm@53 209 (println-repl "hi! <3"))))
rlm@53 210
rlm@53 211
rlm@50 212 (defn test-ragdoll []
rlm@50 213
rlm@50 214 (let [the-worm
rlm@53 215
rlm@52 216 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@53 217 (doto (show-skeleton (worm-blender))
rlm@53 218 (.setLocalTranslation (Vector3f. 0 10 0))
rlm@53 219 ;;(worm)
rlm@52 220 ;;(oto)
rlm@52 221 ;;(sinbad)
rlm@50 222 )
rlm@50 223 ]
rlm@50 224
rlm@50 225
rlm@50 226 (.start
rlm@50 227 (world
rlm@50 228 (doto (Node.)
rlm@50 229 (.attachChild the-worm))
rlm@53 230 {"key-return" (fire-cannon-ball)
rlm@53 231 "key-space" (partial test-zzz the-worm)
rlm@53 232 }
rlm@50 233 (fn [world]
rlm@50 234 (light-up-everything world)
rlm@52 235 (PhysicsTestHelper/createPhysicsTestWorld
rlm@52 236 (.getRootNode world)
rlm@52 237 (asset-manager)
rlm@52 238 (.getPhysicsSpace
rlm@52 239 (.getState (.getStateManager world) BulletAppState)))
rlm@53 240 (set-gravity world Vector3f/ZERO)
rlm@50 241 ;;(.setTimer world (NanoTimer.))
rlm@50 242 ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@50 243 )
rlm@50 244 no-op
rlm@50 245 )
rlm@50 246
rlm@50 247
rlm@50 248 )))
rlm@50 249
rlm@58 250
rlm@58 251
rlm@58 252
rlm@58 253
rlm@58 254 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@58 255 ;;; here is the ragdoll stuff
rlm@58 256
rlm@58 257 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@58 258 (def mesh worm-mesh)
rlm@58 259
rlm@58 260 (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@58 261 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@58 262 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@58 263
rlm@58 264
rlm@58 265 (defn position [index]
rlm@58 266 (.get
rlm@58 267 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@58 268 index))
rlm@58 269
rlm@58 270 (defn bones [index]
rlm@58 271 (.get
rlm@58 272 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@58 273 index))
rlm@58 274
rlm@58 275 (defn bone-weights [index]
rlm@58 276 (.get
rlm@58 277 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@58 278 index))
rlm@58 279
rlm@58 280
rlm@58 281
rlm@58 282 (defn vertex-bones [vertex]
rlm@58 283 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@58 284
rlm@58 285 (defn vertex-weights [vertex]
rlm@58 286 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@58 287
rlm@58 288 (defn vertex-position [index]
rlm@58 289 (let [offset (* index 3)]
rlm@58 290 (Vector3f. (position offset)
rlm@58 291 (position (inc offset))
rlm@58 292 (position (inc(inc offset))))))
rlm@58 293
rlm@58 294 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@58 295
rlm@58 296 (defn bone-control-color [index]
rlm@58 297 (get {[1 0 0 0] ColorRGBA/Red
rlm@58 298 [1 2 0 0] ColorRGBA/Magenta
rlm@58 299 [2 0 0 0] ColorRGBA/Blue}
rlm@58 300 (vertex-bones index)
rlm@58 301 ColorRGBA/White))
rlm@58 302
rlm@58 303 (defn influence-color [index bone-num]
rlm@58 304 (get
rlm@58 305 {(float 0) ColorRGBA/Blue
rlm@58 306 (float 0.5) ColorRGBA/Green
rlm@58 307 (float 1) ColorRGBA/Red}
rlm@58 308 ;; find the weight of the desired bone
rlm@58 309 ((zipmap (vertex-bones index)(vertex-weights index))
rlm@58 310 bone-num)
rlm@58 311 ColorRGBA/Blue))
rlm@58 312
rlm@58 313 (def worm-vertices (set (map vertex-info (range 60))))
rlm@58 314
rlm@58 315
rlm@58 316 (defn test-info []
rlm@58 317 (let [points (Node.)]
rlm@58 318 (dorun
rlm@58 319 (map #(.attachChild points %)
rlm@58 320 (map #(sphere 0.01
rlm@58 321 :position (vertex-position %)
rlm@58 322 :color (influence-color % 1)
rlm@58 323 :physical? false)
rlm@58 324 (range 60))))
rlm@58 325 (view points)))
rlm@58 326
rlm@58 327 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@58 328
rlm@60 329 ;;;;;;;;;;;; eve-style bodies ;;;;;;;;
rlm@58 330
rlm@58 331
rlm@60 332 (defrecord JointControl [joint physics-space]
rlm@60 333 PhysicsControl
rlm@60 334 (setPhysicsSpace [this space]
rlm@60 335 (dosync
rlm@60 336 (ref-set (:physics-space this) space))
rlm@60 337 (.addJoint space (:joint this)))
rlm@60 338 (update [this tpf])
rlm@60 339 (setSpatial [this spatial])
rlm@60 340 (render [this rm vp])
rlm@60 341 (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@60 342 (isEnabled [this] true)
rlm@60 343 (setEnabled [this state]))
rlm@60 344
rlm@55 345 (defn add-joint
rlm@55 346 "Add a joint to a particular object. When the object is added to the
rlm@55 347 PhysicsSpace of a simulation, the joint will also be added"
rlm@55 348 [object joint]
rlm@60 349 (let [control (JointControl. joint (ref nil))]
rlm@55 350 (.addControl object control))
rlm@55 351 object)
rlm@50 352
rlm@55 353 (defn hinge-world
rlm@55 354 []
rlm@55 355 (let [sphere1 (sphere)
rlm@55 356 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@55 357 joint (Point2PointJoint.
rlm@55 358 (.getControl sphere1 RigidBodyControl)
rlm@55 359 (.getControl sphere2 RigidBodyControl)
rlm@55 360 Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@55 361 (add-joint sphere1 joint)
rlm@55 362 (doto (Node. "hinge-world")
rlm@55 363 (.attachChild sphere1)
rlm@55 364 (.attachChild sphere2))))
rlm@55 365
rlm@56 366 (defn test-joint []
rlm@56 367 (view (hinge-world)))
rlm@49 368
rlm@49 369
rlm@58 370 (defn worm [segment-length num-segments interstitial-space radius]
rlm@58 371 (letfn [(nth-segment
rlm@58 372 [n]
rlm@58 373 (box segment-length radius radius :mass 0.1
rlm@58 374 :position
rlm@58 375 (Vector3f.
rlm@58 376 (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@58 377 :name (str "worm-segment" n)
rlm@58 378 :color (ColorRGBA/randomColor)))]
rlm@58 379 (map nth-segment (range num-segments))))
rlm@58 380
rlm@58 381 (defn nodify
rlm@58 382 "take a sequence of things that can be attached to a node and return
rlm@60 383 a node with all of them attached"
rlm@58 384 ([name children]
rlm@58 385 (let [node (Node. name)]
rlm@58 386 (dorun (map #(.attachChild node %) children))
rlm@58 387 node))
rlm@58 388 ([children] (nodify "" children)))
rlm@58 389
rlm@58 390
rlm@58 391 (defn connect-at-midpoint
rlm@58 392 [segmentA segmentB]
rlm@58 393 (let [centerA (.getWorldTranslation segmentA)
rlm@58 394 centerB (.getWorldTranslation segmentB)
rlm@58 395 midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@58 396 pivotA (.subtract midpoint centerA)
rlm@58 397 pivotB (.subtract midpoint centerB)
rlm@58 398
rlm@58 399 joint (Point2PointJoint.
rlm@58 400 (.getControl segmentA RigidBodyControl)
rlm@58 401 (.getControl segmentB RigidBodyControl)
rlm@58 402 pivotA
rlm@58 403 pivotB)]
rlm@58 404 (add-joint segmentA joint)
rlm@58 405 segmentB))
rlm@58 406
rlm@58 407
rlm@58 408 (defn point-worm []
rlm@58 409 (let [segments (worm 0.2 5 0.1 0.1)]
rlm@58 410 (dorun (map (partial apply connect-at-midpoint)
rlm@58 411 (partition 2 1 segments)))
rlm@58 412 (nodify "worm" segments)))
rlm@58 413
rlm@58 414
rlm@58 415 (defn test-worm []
rlm@58 416 (.start
rlm@58 417 (world
rlm@58 418 (doto (Node.)
rlm@58 419 ;;(.attachChild (point-worm))
rlm@58 420 (.attachChild (load-blender-model
rlm@58 421 "Models/anim2/joint-worm.blend"))
rlm@58 422
rlm@58 423 (.attachChild (box 10 1 10
rlm@58 424 :position (Vector3f. 0 -2 0) :mass 0
rlm@58 425 :color (ColorRGBA/Gray))))
rlm@58 426 {
rlm@58 427 "key-space" (fire-cannon-ball)
rlm@58 428 }
rlm@58 429 (fn [world]
rlm@58 430 (enable-debug world)
rlm@58 431 (light-up-everything world)
rlm@58 432 ;;(.setTimer world (NanoTimer.))
rlm@58 433 )
rlm@58 434 no-op)))
rlm@49 435
rlm@56 436 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@56 437
rlm@56 438
rlm@60 439 ;;;;;;;;; Mortor Control ;;;;;;;;;;;;;
rlm@56 440
rlm@56 441
rlm@60 442 ;; surprisingly ehough, terristerial creatures only move by using
rlm@60 443 ;; torque applied to their joints. There's not a single straight line
rlm@60 444 ;; of force in the human body at all! (a straight line of force would
rlm@60 445 ;; correspond to some sort of jet or rocket propulseion)
rlm@56 446
rlm@60 447 (defn node-seq
rlm@60 448 "take a node and return a seq of all its children
rlm@60 449 recursively. There will be no nodes left in the resulting
rlm@60 450 structure"
rlm@60 451 [#^Node node]
rlm@60 452 (tree-seq #(isa? (class %) Node) #(.getChildren %) node))
rlm@60 453
rlm@60 454
rlm@60 455 (defn torque-controls [control]
rlm@60 456 (let [torques
rlm@60 457 (concat
rlm@60 458 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@60 459 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@60 460 [Vector3f/UNIT_X])]
rlm@60 461 (map (fn [torque-axis]
rlm@60 462 (fn [torque]
rlm@60 463 (.applyTorque
rlm@60 464 control
rlm@60 465 (.mult (.mult (.getPhysicsRotation control)
rlm@60 466 torque-axis)
rlm@60 467 (float
rlm@60 468 (* (.getMass control) torque))))))
rlm@60 469 torques)))
rlm@56 470
rlm@60 471 (defn motor-map
rlm@60 472 "Take a creature and generate a function that will enable fine
rlm@60 473 grained control over all the creature's limbs."
rlm@60 474 [#^Node creature]
rlm@60 475 (let [controls (keep #(.getControl % RigidBodyControl)
rlm@60 476 (node-seq creature))
rlm@60 477 limb-controls (reduce concat (map torque-controls controls))
rlm@60 478 body-control (partial map #(%1 %2) limb-controls)]
rlm@60 479 body-control))
rlm@56 480
rlm@56 481
rlm@56 482
rlm@60 483 (defn test-motor-map
rlm@60 484 "see how torque works."
rlm@60 485 []
rlm@60 486 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@60 487 :mass 1 :color ColorRGBA/Green)
rlm@60 488 motor-map (motor-map finger)]
rlm@60 489 (world
rlm@60 490 (nodify [finger
rlm@60 491 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 492 :color ColorRGBA/Gray)])
rlm@60 493 standard-debug-controls
rlm@60 494 (fn [world]
rlm@60 495 (set-gravity world Vector3f/ZERO)
rlm@60 496 (light-up-everything world)
rlm@60 497 (.setTimer world (NanoTimer.)))
rlm@60 498 (fn [_ _]
rlm@60 499 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
rlm@60 500
rlm@60 501
rlm@60 502
rlm@60 503 (defn test-torque
rlm@60 504 "see how torque works."
rlm@60 505 []
rlm@60 506 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@60 507 :mass 1 :color ColorRGBA/Green)
rlm@60 508 move-left? (atom false)
rlm@60 509 move-right? (atom false)
rlm@60 510 control (.getControl finger RigidBodyControl)]
rlm@60 511 (world
rlm@60 512 (nodify [finger
rlm@60 513 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 514 :color ColorRGBA/Gray)])
rlm@60 515 (merge standard-debug-controls
rlm@60 516 {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@60 517 "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
rlm@60 518 (fn [world]
rlm@60 519 (set-gravity world Vector3f/ZERO)
rlm@60 520 (light-up-everything world)
rlm@60 521 (.setTimer world (NanoTimer.)))
rlm@60 522 (fn [_ _]
rlm@60 523 (if @move-left?
rlm@60 524 (.applyTorque control
rlm@60 525 (.mult (.getPhysicsRotation control)
rlm@60 526 (Vector3f. -3 20 0))))
rlm@60 527 (if @move-right?
rlm@60 528 (.applyTorque control (Vector3f. 0 0 1)))))))
rlm@60 529
rlm@60 530
rlm@60 531 (defn worm-pattern [time]
rlm@60 532 [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60 533
rlm@60 534 0 0 0 0 0 0 0 0 0 0 0
rlm@60 535
rlm@60 536 (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
rlm@60 537
rlm@60 538 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60 539 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
rlm@60 540 0 0 0 0 0 0 0 0 0 0 0 0 0 0
rlm@60 541
rlm@60 542 ])
rlm@60 543
rlm@60 544
rlm@60 545 ;; (defn copier-gen []
rlm@60 546 ;; (let [count (atom 0)]
rlm@60 547 ;; (fn [in]
rlm@60 548 ;; (swap! count inc)
rlm@60 549 ;; (clojure.contrib.duck-streams/copy
rlm@60 550 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@60 551 ;; ;;/home/r/tmp/mao-test/clojure-images
rlm@60 552 ;; (format "%08d.png" @count)))))))
rlm@60 553 ;; (defn decrease-framerate []
rlm@60 554 ;; (map
rlm@60 555 ;; (copier-gen)
rlm@60 556 ;; (sort
rlm@60 557 ;; (map first
rlm@60 558 ;; (partition
rlm@60 559 ;; 4
rlm@60 560 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@60 561 ;; (file-seq
rlm@60 562 ;; (file-str
rlm@60 563 ;; "/home/r/media/anime/mao-temp/images"))))))))
rlm@60 564
rlm@60 565
rlm@60 566
rlm@60 567 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60 568
rlm@60 569
rlm@60 570
rlm@60 571
rlm@60 572 ;;;;;;;;;;;;;;;;;; Proprioception ;;;;;;;;;;;;;;;;;;;;;;;;
rlm@60 573
rlm@60 574
rlm@60 575 ;; this is not used as just getting the rotation would be simpler.
rlm@60 576 (defn proprioception-senses
rlm@60 577 "given a control , create a sequence of thunks that will report the
rlm@60 578 rotation of the control's object along the same axes as the motor-control map."
rlm@60 579 [control]
rlm@60 580 (let [torques
rlm@60 581 (concat
rlm@60 582 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@60 583 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@60 584 [Vector3f/UNIT_X])]
rlm@60 585 (map (fn [torque-axis]
rlm@60 586 (fn []
rlm@60 587 (.getPhysicsRotation control)))
rlm@60 588 torques)))
rlm@60 589
rlm@60 590 (defn orthogonal-vect
rlm@60 591 "Return a vector orthogonal to the current one"
rlm@60 592 [vector]
rlm@60 593 (let [x (.getX vector)
rlm@60 594 y (.getY vector)
rlm@60 595 z (.getZ vector)]
rlm@60 596 (cond
rlm@60 597 (not= x (float 0)) (Vector3f. (- z) 0 x)
rlm@60 598 (not= y (float 0)) (Vector3f. 0 (- z) y)
rlm@60 599 (not= z (float 0)) (Vector3f. 0 (- z) y)
rlm@60 600 true Vector3f/ZERO)))
rlm@60 601
rlm@60 602 ;; from
rlm@60 603 ;; http://stackoverflow.com/questions/3684269/ \\
rlm@60 604 ;; component-of-a-quaternion-rotation-around-an-axis
rlm@60 605 (defn rot-about-axis [#^Quaternion q #^Vector3f axis]
rlm@60 606 (let [basis-1 (orthogonal-vect axis)
rlm@60 607 basis-2 (.cross axis basis-1)
rlm@60 608 rotated (.mult q basis-1)
rlm@60 609 alpha (.dot basis-1 (.project rotated basis-1))
rlm@60 610 beta (.dot basis-2 (.project rotated basis-2))]
rlm@60 611 (println-repl alpha)
rlm@60 612 (println-repl beta)
rlm@60 613 (Math/atan2 beta alpha)))
rlm@60 614
rlm@60 615
rlm@60 616 (defn check-rot [a]
rlm@60 617 (rot-about-axis
rlm@60 618 (doto (Quaternion.)
rlm@60 619 (.fromAngleAxis
rlm@60 620 (float a)
rlm@60 621 (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
rlm@60 622
rlm@60 623 (defn relative-positions [joint]
rlm@60 624 (let [object-a (.getUserObject (.getBodyA joint))
rlm@60 625 object-b (.getUserObject (.getBodyB joint))
rlm@60 626 arm-a
rlm@60 627 (.normalize
rlm@60 628 (.subtract
rlm@60 629 (.localToWorld object-a (.getPivotA joint) nil)
rlm@60 630 (.getWorldTranslation object-a)))
rlm@60 631 rotate-a
rlm@60 632 (doto (Matrix3f.)
rlm@60 633 (.fromStartEndVectors arm-a Vector3f/UNIT_X))
rlm@60 634 arm-b
rlm@60 635 (.mult
rlm@60 636 rotate-a
rlm@60 637 (.normalize
rlm@60 638 (.subtract
rlm@60 639 (.localToWorld object-b (.getPivotB joint) nil)
rlm@60 640 (.getWorldTranslation object-b))))
rlm@60 641 rotate-b
rlm@60 642 (doto (Matrix3f.)
rlm@60 643 (.fromStartEndVectors arm-b Vector3f/UNIT_X))
rlm@60 644
rlm@60 645 pitch
rlm@60 646 (.angleBetween
rlm@60 647 (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
rlm@60 648 (Vector2f. 1 0))
rlm@60 649 yaw
rlm@60 650 (.angleBetween
rlm@60 651 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
rlm@60 652 (Vector2f. 1 0))
rlm@60 653
rlm@60 654 roll
rlm@60 655 (.mult
rlm@60 656 (.getLocalRotation object-b)
rlm@60 657 (doto (Quaternion.)
rlm@60 658 (.fromRotationMatrix rotate-a)))
rlm@60 659 ]
rlm@60 660
rlm@60 661
rlm@60 662
rlm@60 663 ;;(println-repl
rlm@60 664 ;; "arm-b is " arm-b)
rlm@60 665 ;;(println-repl
rlm@60 666 ;; "pivot-b is " (.getPivotB joint))
rlm@60 667 ;;(println-repl
rlm@60 668 ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@60 669 ;; pitch yaw roll))
rlm@60 670 [pitch yaw roll]))
rlm@60 671
rlm@60 672
rlm@60 673
rlm@60 674
rlm@60 675
rlm@60 676 (defn proprioception
rlm@60 677 "Create a proprioception map that reports the rotations of the
rlm@60 678 various limbs of the creature's body"
rlm@60 679 [creature]
rlm@60 680 [#^Node creature]
rlm@60 681 (let [
rlm@60 682 nodes (node-seq creature)
rlm@60 683 joints
rlm@60 684 (map
rlm@60 685 :joint
rlm@60 686 (filter
rlm@60 687 #(isa? (class %) JointControl)
rlm@60 688 (reduce
rlm@60 689 concat
rlm@60 690 (map (fn [node]
rlm@60 691 (map (fn [num] (.getControl node num))
rlm@60 692 (range (.getNumControls node))))
rlm@60 693 nodes))))]
rlm@60 694 (fn []
rlm@60 695 (reduce concat (map relative-positions (list (first joints)))))))
rlm@60 696
rlm@60 697
rlm@60 698 (defn test-worm-control
rlm@60 699 []
rlm@60 700 (let [worm (point-worm)
rlm@60 701 time (atom 0)
rlm@60 702 worm-motor-map (motor-map worm)
rlm@60 703 body-map (proprioception worm)
rlm@60 704 debug-segments
rlm@60 705 (map
rlm@60 706 #(doto
rlm@60 707 (make-shape
rlm@60 708 (assoc base-shape
rlm@60 709 :name "debug-line"
rlm@60 710 :physical? false
rlm@60 711 :shape
rlm@60 712 (com.jme3.scene.shape.Line.
rlm@60 713 (.add (.getWorldTranslation %)
rlm@60 714 (Vector3f. -0.2 0 0 ))
rlm@60 715 (.add (.getWorldTranslation %)
rlm@60 716 (Vector3f. 0.2 0 0)))))
rlm@60 717 (.setMaterial (green-x-ray)))
rlm@60 718 (drop 1 (node-seq worm)))]
rlm@60 719 (world
rlm@60 720 (nodify [worm
rlm@60 721 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 722 :color ColorRGBA/Gray)])
rlm@60 723 standard-debug-controls
rlm@60 724 (fn [world]
rlm@60 725 (.attachChild (.getRootNode world) (nodify debug-segments))
rlm@60 726 (enable-debug world)
rlm@60 727 (light-up-everything world)
rlm@60 728 (com.aurellem.capture.Capture/captureVideo
rlm@60 729 world
rlm@60 730 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@60 731
rlm@60 732 (fn [_ _]
rlm@60 733 (dorun
rlm@60 734 (map
rlm@60 735 (fn [worm-segment
rlm@60 736 debug-segment]
rlm@60 737 (.rotate
rlm@60 738 debug-segment
rlm@60 739 (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
rlm@60 740 (drop 1 (node-seq worm))
rlm@60 741 debug-segments))
rlm@60 742 (swap! time inc)
rlm@60 743 (println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))
rlm@60 744 (Thread/sleep 200)
rlm@60 745 (dorun (worm-motor-map
rlm@60 746 (worm-pattern @time)))))))
rlm@60 747
rlm@60 748
rlm@60 749
rlm@60 750
rlm@60 751
rlm@60 752 (defn test-prop
rlm@60 753 "see how torque works."
rlm@60 754 []
rlm@60 755 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
rlm@60 756 :mass 0 :color ColorRGBA/Green)
rlm@60 757 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
rlm@60 758 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
rlm@60 759 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0)
rlm@60 760 :mass 0 :color ColorRGBA/Gray)
rlm@60 761
rlm@60 762 move-up? (atom false)
rlm@60 763 move-down? (atom false)
rlm@60 764 move-left? (atom false)
rlm@60 765 move-right? (atom false)
rlm@60 766 roll-left? (atom false)
rlm@60 767 roll-right? (atom false)
rlm@60 768 control (.getControl finger RigidBodyControl)
rlm@60 769 joint
rlm@60 770 (doto
rlm@60 771 (Point2PointJoint.
rlm@60 772 (.getControl hand RigidBodyControl)
rlm@60 773 control
rlm@60 774 (Vector3f. 1.2 0 0)
rlm@60 775 (Vector3f. -1.2 0 0 ))
rlm@60 776 (.setCollisionBetweenLinkedBodys false))
rlm@60 777 time (atom 0)
rlm@60 778 ]
rlm@60 779 (world
rlm@60 780 (nodify [hand finger floor])
rlm@60 781 (merge standard-debug-controls
rlm@60 782 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
rlm@60 783 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
rlm@60 784 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@60 785 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
rlm@60 786 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
rlm@60 787 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
rlm@60 788 (fn [world]
rlm@60 789 (set-gravity world Vector3f/ZERO)
rlm@60 790 (.setMoveSpeed (.getFlyByCamera world) 50)
rlm@60 791 (.setRotationSpeed (.getFlyByCamera world) 50)
rlm@60 792 (light-up-everything world)
rlm@60 793 (.setTimer world (NanoTimer.))
rlm@60 794 )
rlm@60 795 (fn [_ _]
rlm@60 796 (if @move-up?
rlm@60 797 (.applyTorque control
rlm@60 798 (.mult (.getPhysicsRotation control)
rlm@60 799 (Vector3f. 0 0 1))))
rlm@60 800 (if @move-down?
rlm@60 801 (.applyTorque control
rlm@60 802 (.mult (.getPhysicsRotation control)
rlm@60 803 (Vector3f. 0 0 -1))))
rlm@60 804 (if @move-left?
rlm@60 805 (.applyTorque control
rlm@60 806 (.mult (.getPhysicsRotation control)
rlm@60 807 (Vector3f. 0 1 0))))
rlm@60 808 (if @move-right?
rlm@60 809 (.applyTorque control
rlm@60 810 (.mult (.getPhysicsRotation control)
rlm@60 811 (Vector3f. 0 -1 0))))
rlm@60 812 (if @roll-left?
rlm@60 813 (.applyTorque control
rlm@60 814 (.mult (.getPhysicsRotation control)
rlm@60 815 (Vector3f. -0.1 0 0))))
rlm@60 816 (if @roll-right?
rlm@60 817 (.applyTorque control
rlm@60 818 (.mult (.getPhysicsRotation control)
rlm@60 819 (Vector3f. 0.1 0 0))))
rlm@60 820
rlm@60 821 (if (= 0 (rem (swap! time inc) 2000))
rlm@60 822 (do
rlm@60 823 ;;(println-repl (.getLocalRotation finger))
rlm@60 824 (println-repl (nth (relative-positions joint) 2))))
rlm@60 825
rlm@60 826 ))))
rlm@60 827
rlm@60 828
rlm@60 829
rlm@60 830
rlm@60 831 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@56 832
rlm@56 833
rlm@56 834
rlm@56 835
rlm@0 836 #+end_src
rlm@0 837
rlm@60 838 #+results: body-main
rlm@60 839 : #'cortex.body/test-prop
rlm@60 840
rlm@0 841
rlm@0 842
rlm@0 843
rlm@0 844
rlm@0 845
rlm@0 846
rlm@0 847
rlm@0 848 * COMMENT generate Source.
rlm@44 849 #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@0 850 <<body-main>>
rlm@0 851 #+end_src
rlm@0 852
rlm@60 853