cortex: org/body.org comparison

comparison org/body.org @ 60:e5e627f50a3a

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

comparison

equal deleted inserted replaced

-:63951929fe44
+:e5e627f50a3a
 (.setMaterial (green-x-ray)))]
 (.attachChild node sd)
 node))
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;; this could be a good way to give objects special properties like
+;; being eyes and the like
+(.getUserData
+(.getChild
+(load-blender-model "Models/property/test.blend") 0)
+"properties")
+;; the properties are saved along with the blender file.
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 (defn init-debug-skel-node
 [f debug-node skeleton]
 (let [bones
 (map #(.getBone skeleton %)
 (range 60))))
 (view points)))
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;;;;;;;;;;;  eve-style bodies  ;;;;;;;;
-(defn joint-control
-[joint]
-(let [physics-space (ref nil)]
+(defrecord JointControl [joint physics-space]
-(reify PhysicsControl
+PhysicsControl
 (setPhysicsSpace [this space]
 (dosync
-(ref-set physics-space space))
+(ref-set (:physics-space this) space))
-(.addJoint space joint))
+(.addJoint space (:joint this)))
 (update [this tpf])
 (setSpatial [this spatial])
 (render [this rm vp])
-(getPhysicsSpace [this] (deref physics-space))
+(getPhysicsSpace [this] (deref (:physics-space this)))
 (isEnabled [this] true)
-(setEnabled [this state]))))
+(setEnabled [this state]))
 (defn add-joint
 "Add a joint to a particular object. When the object is added to the
 PhysicsSpace of a simulation, the joint will also be added"
 [object joint]
-(let [control (joint-control joint)]
+(let [control (JointControl. joint (ref nil))]
 (.addControl object control))
 object)
 (defn hinge-world
 []
 :color (ColorRGBA/randomColor)))]
 (map nth-segment (range num-segments))))
 (defn nodify
 "take a sequence of things that can be attached to a node and return
-a node with all of the attached"
+a node with all of them attached"
 ([name children]
 (let [node (Node. name)]
 (dorun (map #(.attachChild node %) children))
 node))
 ([children] (nodify "" children)))
 no-op)))
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;;;;;;;;  Mortor Control  ;;;;;;;;;;;;;
+;; surprisingly ehough, terristerial creatures only move by using
+;; torque applied to their joints.  There's not a single straight line
+;; of force in the human body at all!  (a straight line of force would
+;; correspond to some sort of jet or rocket propulseion)
+(defn node-seq
+"take a node and return a seq of all its children
+recursively. There will be no nodes left in the resulting
+structure"
+[#^Node node]
+(tree-seq #(isa? (class %) Node) #(.getChildren %) node))
+(defn torque-controls [control]
+(let [torques
+(concat
+(map #(Vector3f. 0 (Math/sin %) (Math/cos %))
+(range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
+[Vector3f/UNIT_X])]
+(map (fn [torque-axis]
+(fn [torque]
+(.applyTorque
+control
+(.mult (.mult (.getPhysicsRotation control)
+torque-axis)
+(float
+(* (.getMass control) torque))))))
+torques)))
+(defn motor-map
+"Take a creature and generate a function that will enable fine
+grained control over all the creature's limbs."
+[#^Node creature]
+(let [controls (keep #(.getControl % RigidBodyControl)
+(node-seq creature))
+limb-controls (reduce concat (map torque-controls controls))
+body-control  (partial map #(%1 %2) limb-controls)]
+body-control))
+(defn test-motor-map
+"see how torque works."
+[]
+(let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
+:mass 1 :color ColorRGBA/Green)
+motor-map (motor-map finger)]
+(world
+(nodify [finger
+(box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+:color ColorRGBA/Gray)])
+standard-debug-controls
+(fn [world]
+(set-gravity world Vector3f/ZERO)
+(light-up-everything world)
+(.setTimer world (NanoTimer.)))
+(fn [_ _]
+(dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
+(defn test-torque
+"see how torque works."
+[]
+(let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
+:mass 1 :color ColorRGBA/Green)
+move-left? (atom false)
+move-right? (atom false)
+control (.getControl finger RigidBodyControl)]
+(world
+(nodify [finger
+(box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+:color ColorRGBA/Gray)])
+(merge standard-debug-controls
+{"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
+"key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
+(fn [world]
+(set-gravity world Vector3f/ZERO)
+(light-up-everything world)
+(.setTimer world (NanoTimer.)))
+(fn [_ _]
+(if @move-left?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. -3 20 0))))
+(if @move-right?
+(.applyTorque control (Vector3f. 0 0 1)))))))
+(defn worm-pattern [time]
+[ 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
+(* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
+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 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 0 0
+])
+;; (defn copier-gen []
+;;   (let [count (atom 0)]
+;;     (fn [in]
+;;       (swap! count inc)
+;;       (clojure.contrib.duck-streams/copy
+;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"
+;;                      ;;/home/r/tmp/mao-test/clojure-images
+;;                (format "%08d.png" @count)))))))
+;; (defn decrease-framerate []
+;;   (map
+;;    (copier-gen)
+;;    (sort
+;;     (map first
+;;          (partition
+;;           4
+;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))
+;;                   (file-seq
+;;                    (file-str
+;;                     "/home/r/media/anime/mao-temp/images"))))))))
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;;;;;;;;;;;;;;;;;  Proprioception  ;;;;;;;;;;;;;;;;;;;;;;;;
+;; this is not used as just getting the rotation would be simpler.
+(defn proprioception-senses
+"given a control , create a sequence of thunks that will report the
+rotation of the control's object along the same axes as the motor-control map."
+[control]
+(let [torques
+(concat
+(map #(Vector3f. 0 (Math/sin %) (Math/cos %))
+(range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
+[Vector3f/UNIT_X])]
+(map (fn [torque-axis]
+(fn []
+(.getPhysicsRotation control)))
+torques)))
+(defn orthogonal-vect
+"Return a vector orthogonal to the current one"
+[vector]
+(let [x (.getX vector)
+y (.getY vector)
+z (.getZ vector)]
+(cond
+(not= x (float 0)) (Vector3f. (- z) 0 x)
+(not= y (float 0)) (Vector3f. 0 (- z) y)
+(not= z (float 0)) (Vector3f. 0 (- z) y)
+true Vector3f/ZERO)))
+;; from
+;; http://stackoverflow.com/questions/3684269/ \\
+;;   component-of-a-quaternion-rotation-around-an-axis
+(defn rot-about-axis [#^Quaternion q #^Vector3f axis]
+(let [basis-1 (orthogonal-vect axis)
+basis-2 (.cross axis basis-1)
+rotated (.mult q basis-1)
+alpha (.dot basis-1 (.project rotated basis-1))
+beta  (.dot basis-2 (.project rotated basis-2))]
+(println-repl alpha)
+(println-repl beta)
+(Math/atan2 beta alpha)))
+(defn check-rot [a]
+(rot-about-axis
+(doto (Quaternion.)
+(.fromAngleAxis
+(float a)
+(Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
+(defn relative-positions [joint]
+(let [object-a (.getUserObject (.getBodyA joint))
+object-b (.getUserObject (.getBodyB joint))
+arm-a
+(.normalize
+(.subtract
+(.localToWorld object-a (.getPivotA joint) nil)
+(.getWorldTranslation object-a)))
+rotate-a
+(doto (Matrix3f.)
+(.fromStartEndVectors arm-a Vector3f/UNIT_X))
+arm-b
+(.mult
+rotate-a
+(.normalize
+(.subtract
+(.localToWorld object-b (.getPivotB joint) nil)
+(.getWorldTranslation object-b))))
+rotate-b
+(doto (Matrix3f.)
+(.fromStartEndVectors arm-b Vector3f/UNIT_X))
+pitch
+(.angleBetween
+(.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
+(Vector2f. 1 0))
+yaw
+(.angleBetween
+(.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
+(Vector2f. 1 0))
+roll
+(.mult
+(.getLocalRotation object-b)
+(doto (Quaternion.)
+(.fromRotationMatrix rotate-a)))
+]
+;;(println-repl
+;; "arm-b is " arm-b)
+;;(println-repl
+;; "pivot-b is " (.getPivotB joint))
+;;(println-repl
+;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
+;;         pitch yaw roll))
+[pitch yaw roll]))
+(defn proprioception
+"Create a proprioception map that reports the rotations of the
+various limbs of the creature's body"
+[creature]
+[#^Node creature]
+(let [
+nodes (node-seq creature)
+joints
+(map
+:joint
+(filter
+#(isa? (class %) JointControl)
+(reduce
+concat
+(map (fn [node]
+(map (fn [num] (.getControl node num))
+(range (.getNumControls node))))
+nodes))))]
+(fn []
+(reduce concat (map relative-positions (list (first joints)))))))
+(defn test-worm-control
+[]
+(let [worm (point-worm)
+time (atom 0)
+worm-motor-map (motor-map worm)
+body-map (proprioception worm)
+debug-segments
+(map
+#(doto
+(make-shape
+(assoc base-shape
+:name "debug-line"
+:physical? false
+:shape
+(com.jme3.scene.shape.Line.
+(.add (.getWorldTranslation %)
+(Vector3f. -0.2 0 0 ))
+(.add (.getWorldTranslation %)
+(Vector3f. 0.2 0 0)))))
+(.setMaterial (green-x-ray)))
+(drop 1 (node-seq worm)))]
+(world
+(nodify [worm
+(box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
+:color ColorRGBA/Gray)])
+standard-debug-controls
+(fn [world]
+(.attachChild (.getRootNode world) (nodify debug-segments))
+(enable-debug world)
+(light-up-everything world)
+(com.aurellem.capture.Capture/captureVideo
+world
+(file-str "/home/r/proj/cortex/tmp/moving-worm")))
+(fn [_ _]
+(dorun
+(map
+(fn [worm-segment
+debug-segment]
+(.rotate
+debug-segment
+(Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
+(drop 1 (node-seq worm))
+debug-segments))
+(swap! time inc)
+(println-repl (with-out-str (clojure.pprint/pprint (doall (body-map)))))
+(Thread/sleep 200)
+(dorun (worm-motor-map
+(worm-pattern @time)))))))
+(defn test-prop
+"see how torque works."
+[]
+(let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
+:mass 0 :color ColorRGBA/Green)
+finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
+:mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
+floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
+:mass 0 :color ColorRGBA/Gray)
+move-up? (atom false)
+move-down? (atom false)
+move-left? (atom false)
+move-right? (atom false)
+roll-left? (atom false)
+roll-right? (atom false)
+control (.getControl finger RigidBodyControl)
+joint
+(doto
+(Point2PointJoint.
+(.getControl hand RigidBodyControl)
+control
+(Vector3f. 1.2 0 0)
+(Vector3f. -1.2 0 0 ))
+(.setCollisionBetweenLinkedBodys false))
+time (atom 0)
+]
+(world
+(nodify [hand finger floor])
+(merge standard-debug-controls
+{"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
+"key-t" (fn [_ pressed?] (reset! move-down? pressed?))
+"key-f" (fn [_ pressed?] (reset! move-left? pressed?))
+"key-g" (fn [_ pressed?] (reset! move-right? pressed?))
+"key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
+"key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
+(fn [world]
+(set-gravity world Vector3f/ZERO)
+(.setMoveSpeed (.getFlyByCamera world) 50)
+(.setRotationSpeed (.getFlyByCamera world) 50)
+(light-up-everything world)
+(.setTimer world (NanoTimer.))
+)
+(fn [_ _]
+(if @move-up?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. 0 0 1))))
+(if @move-down?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. 0 0 -1))))
+(if @move-left?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. 0 1 0))))
+(if @move-right?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. 0 -1 0))))
+(if @roll-left?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. -0.1 0 0))))
+(if @roll-right?
+(.applyTorque control
+(.mult (.getPhysicsRotation control)
+(Vector3f. 0.1 0 0))))
+(if (= 0 (rem (swap! time inc) 2000))
+(do
+;;(println-repl (.getLocalRotation finger))
+(println-repl (nth (relative-positions joint) 2))))
+))))
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 #+end_src
+#+results: body-main
+: #'cortex.body/test-prop
 * COMMENT generate Source.
 #+begin_src clojure :tangle ../src/cortex/body.clj
 <<body-main>>
 #+end_src

Mercurial > cortex

comparison org/body.org @ 60:e5e627f50a3a