Mercurial > cortex

comparison org/test-creature.org @ 135:421cc43441ae

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cleaned up test, moved some functions
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 05:43:51 -0700
parents ac350a0ac6b0
children ffbab4199c0d
comparison
equal deleted inserted replaced
134:ac350a0ac6b0 135:421cc43441ae
260 [#^Vector3f in] 260 [#^Vector3f in]
261 (Vector3f. (.getX in) 261 (Vector3f. (.getX in)
262 (.getZ in) 262 (.getZ in)
263 (- (.getY in)))) 263 (- (.getY in))))
264 264
265 (defn jme-to-blender 265
266 "Convert from JME coordinates to Blender coordinates"
267 [#^Vector3f in]
268 (Vector3f. (.getX in)
269 (- (.getZ in))
270 (.getY in)))
271
272 (defn joint-targets
273 "Return the two closest two objects to the joint object, ordered
274 from bottom to top according to the joint's rotation."
275 [#^Node parts #^Node joint]
276 (loop [radius (float 0.01)]
277 (let [results (CollisionResults.)]
278 (.collideWith
279 parts
280 (BoundingBox. (.getWorldTranslation joint)
281 radius radius radius)
282 results)
283 (let [targets
284 (distinct
285 (map #(.getGeometry %) results))]
286 (if (>= (count targets) 2)
287 (sort-by
288 #(let [v
289 (jme-to-blender
290 (.mult
291 (.inverse (.getWorldRotation joint))
292 (.subtract (.getWorldTranslation %)
293 (.getWorldTranslation joint))))]
294 (println-repl (.getName %) ":" v)
295 (.dot (Vector3f. 1 1 1)
296 v))
297 (take 2 targets))
298 (recur (float (* radius 2))))))))
299
300
301 (defn proprio-joint [#^Node parts #^Node joint]
302 (let [[obj-a obj-b] (joint-targets parts joint)
303 joint-rot (.getWorldRotation joint)
304 x (.mult joint-rot Vector3f/UNIT_X)
305 y (.mult joint-rot Vector3f/UNIT_Y)
306 z (.mult joint-rot Vector3f/UNIT_Z)]
307 ;; this function will report proprioceptive information for the
308 ;; joint
309 (fn []
310 ;; x is the "twist" axis, y and z are the "bend" axes
311 (let [rot-a (.getWorldRotation obj-a)
312 rot-b (.getWorldRotation obj-b)
313 relative (.mult (.inverse rot-a) rot-b)
314 basis (doto (Matrix3f.)
315 (.setColumn 0 y)
316 (.setColumn 1 z)
317 (.setColumn 2 x))
318 rotation-about-joint
319 (doto (Quaternion.)
320 (.fromRotationMatrix
321 (.mult (.inverse basis)
322 (.toRotationMatrix relative))))
323
324 confirm-axes
325 (let [temp-axes (make-array Vector3f 3)]
326 (.toAxes rotation-about-joint temp-axes)
327 (seq temp-axes))
328 euler-angles
329 (seq (.toAngles rotation-about-joint nil))]
330 ;;return euler angles of the quaternion around the new basis
331 euler-angles))))
332 266
333 267
334 268
335 (defn world-to-local 269 (defn world-to-local
336 "Convert the world coordinates into coordinates relative to the 270 "Convert the world coordinates into coordinates relative to the
1056 990
1057 ;; higher level --- sense/effector 991 ;; higher level --- sense/effector
1058 ;; these are the functions that provide world i/o, chinese-room style 992 ;; these are the functions that provide world i/o, chinese-room style
1059 993
1060 994
1061 (defn creature-joints
1062 "Return the children of the creature's \"joints\" node."
1063 [#^Node creature]
1064 (if-let [joint-node (.getChild creature "joints")]
1065 (seq (.getChildren joint-node))
1066 (do (println-repl "could not find JOINTS node") [])))
1067 995
1068 996
1069 (defn blender-creature 997 (defn blender-creature
1070 "Return a creature with all joints in place." 998 "Return a creature with all joints in place."
1071 [blender-path] 999 [blender-path]
1152 (let [width (.getWidth image)] 1080 (let [width (.getWidth image)]
1153 (for [x (range width)] 1081 (for [x (range width)]
1154 (bit-and 1082 (bit-and
1155 0xFF 1083 0xFF
1156 (.getRGB image x 0))))) 1084 (.getRGB image x 0)))))
1157
1158
1159 (defn rad->deg [rad]
1160 (* 180 (/ Math/PI) rad))
1161
1162
1163 (defn debug-prop-window
1164 "create a debug view for proprioception"
1165 []
1166 (let [vi (view-image)]
1167 (fn [sensor-data]
1168 (println-repl
1169 (map
1170 (fn [[yaw pitch roll]]
1171 [(rad->deg yaw)
1172 (rad->deg pitch)
1173 (rad->deg roll)])
1174 sensor-data)))))
1175 1085
1176 1086
1177 (defn draw-sprite [image sprite x y color ] 1087 (defn draw-sprite [image sprite x y color ]
1178 (dorun 1088 (dorun
1179 (for [[u v] sprite] 1089 (for [[u v] sprite]
1237 hearing-windows (map (fn [_] (debug-hearing-window 50)) 1147 hearing-windows (map (fn [_] (debug-hearing-window 50))
1238 hearing-senses) 1148 hearing-senses)
1239 bell (AudioNode. (asset-manager) 1149 bell (AudioNode. (asset-manager)
1240 "Sounds/pure.wav" false) 1150 "Sounds/pure.wav" false)
1241 prop (proprioception creature) 1151 prop (proprioception creature)
1242 prop-debug (debug-prop-window) 1152 prop-debug (proprioception-debug-window)
1243 ;; dream 1153 ;; dream
1244 1154
1245 ] 1155 ]
1246 (world 1156 (world
1247 (nodify [creature 1157 (nodify [creature