Mercurial > cortex

comparison org/body.org @ 157:84c67be00abe

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
refactored proprioception
author Robert McIntyre <rlm@mit.edu>
date Fri, 03 Feb 2012 06:21:39 -0700
parents e1232043656a
children 33278bf028e7
comparison
equal deleted inserted replaced
156:e8df6e76c3e5 157:84c67be00abe
59 "Return the children of the creature's \"joints\" node." 59 "Return the children of the creature's \"joints\" node."
60 [#^Node creature] 60 [#^Node creature]
61 (if-let [joint-node (.getChild creature "joints")] 61 (if-let [joint-node (.getChild creature "joints")]
62 (seq (.getChildren joint-node)) 62 (seq (.getChildren joint-node))
63 (do (println-repl "could not find JOINTS node") []))) 63 (do (println-repl "could not find JOINTS node") [])))
64
65 (defn right-handed? [vec1 vec2 vec3]
66 (< 0 (.dot (.cross vec1 vec2) vec3)))
67
68 (defn absolute-angle [vec1 vec2 axis]
69 (let [angle (.angleBetween vec1 vec2)]
70 (if (right-handed? vec1 vec2 axis)
71 angle (- (* 2 Math/PI) angle))))
72
73
74 (defn joint-proprioception [#^Node parts #^Node joint]
75 (let [[obj-a obj-b] (joint-targets parts joint)
76 joint-rot (.getWorldRotation joint)
77 x0 (.mult joint-rot Vector3f/UNIT_X)
78 y0 (.mult joint-rot Vector3f/UNIT_Y)
79 z0 (.mult joint-rot Vector3f/UNIT_Z)]
80 (println-repl "x:" x0)
81 (println-repl "y:" y0)
82 (println-repl "z:" z0)
83 (println-repl "init-a:" (.getWorldRotation obj-a))
84 (println-repl "init-b:" (.getWorldRotation obj-b))
85
86 (fn []
87 (let [rot-a (.clone (.getWorldRotation obj-a))
88 rot-b (.clone (.getWorldRotation obj-b))
89 x (.mult rot-a x0)
90 y (.mult rot-a y0)
91 z (.mult rot-a z0)
92
93 X (.mult rot-b x0)
94 Y (.mult rot-b y0)
95 Z (.mult rot-b z0)
96 heading (Math/atan2 (.dot X z) (.dot X x))
97 pitch (Math/atan2 (.dot X y) (.dot X x))
98
99 ;; rotate x-vector back to origin
100 reverse
101 (doto (Quaternion.)
102 (.fromAngleAxis
103 (.angleBetween X x)
104 (let [cross (.normalize (.cross X x))]
105 (if (= 0 (.length cross)) y cross))))
106 roll (absolute-angle (.mult reverse Y) y x)]
107
108 [heading pitch roll]))))
109
110 (defn proprioception
111 "Create a function that provides proprioceptive information about an
112 entire body."
113 [#^Node creature]
114 ;; extract the body's joints
115 (let [joints (creature-joints creature)
116 senses (map (partial joint-proprioception creature) joints)]
117 (fn []
118 (map #(%) senses))))
119 64
120 (defn tap [obj direction force] 65 (defn tap [obj direction force]
121 (let [control (.getControl obj RigidBodyControl)] 66 (let [control (.getControl obj RigidBodyControl)]
122 (.applyTorque 67 (.applyTorque
123 control 68 control