Mercurial > cortex

comparison org/proprioception.org @ 173:1943b3f581c2

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
renamed proprioception functions
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 06:26:40 -0700
parents 84c67be00abe
children 136349ac6972
comparison
equal deleted inserted replaced
172:04a39e6bb695 173:1943b3f581c2
7 #+INCLUDE: ../../aurellem/org/level-0.org 7 #+INCLUDE: ../../aurellem/org/level-0.org
8 8
9 #+name: proprioception 9 #+name: proprioception
10 #+begin_src clojure 10 #+begin_src clojure
11 (ns cortex.proprioception 11 (ns cortex.proprioception
12 (:use (cortex world util sense body))) 12 "Simulate the sense of proprioception (ability to detect the
13 relative positions of body parts with repsect to other body parts)
14 in jMonkeyEngine3. Reads specially prepared blender files to
15 automatically generate proprioceptive senses."
16 (:use (cortex world util sense body))
17 (:use clojure.contrib.def))
18
13 19
14 (cortex.import/mega-import-jme3) 20 (cortex.import/mega-import-jme3)
15 21
16 (defn right-handed? [vec1 vec2 vec3] 22 (defvar
23 ^{:arglists '([creature])}
24 joints
25 (sense-nodes "joints")
26 "Return the children of the creature's \"joints\" node.")
27
28 (defn right-handed?
29 "true iff the three vectors form a right handed coordinate
30 system. The three vectors do not have to be normalized or
31 orthogonal."
32 [vec1 vec2 vec3]
17 (< 0 (.dot (.cross vec1 vec2) vec3))) 33 (< 0 (.dot (.cross vec1 vec2) vec3)))
18 34
19 (defn absolute-angle [vec1 vec2 axis] 35 (defn absolute-angle
36 "The angle between 'vec1 and 'vec2. Positive if the angle to get
37 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
38 otherwise."
39 [vec1 vec2 axis]
20 (let [angle (.angleBetween vec1 vec2)] 40 (let [angle (.angleBetween vec1 vec2)]
21 (if (right-handed? vec1 vec2 axis) 41 (if (right-handed? vec1 vec2 axis)
22 angle (- (* 2 Math/PI) angle)))) 42 angle (- (* 2 Math/PI) angle))))
23 43
24 44 (defn proprioception-fn
25 (defn joint-proprioception [#^Node parts #^Node joint] 45 "Returns a function which returns proprioceptive sensory data when
46 called inside a running simulation."
47 [#^Node parts #^Node joint]
26 (let [[obj-a obj-b] (joint-targets parts joint) 48 (let [[obj-a obj-b] (joint-targets parts joint)
27 joint-rot (.getWorldRotation joint) 49 joint-rot (.getWorldRotation joint)
28 x0 (.mult joint-rot Vector3f/UNIT_X) 50 x0 (.mult joint-rot Vector3f/UNIT_X)
29 y0 (.mult joint-rot Vector3f/UNIT_Y) 51 y0 (.mult joint-rot Vector3f/UNIT_Y)
30 z0 (.mult joint-rot Vector3f/UNIT_Z)] 52 z0 (.mult joint-rot Vector3f/UNIT_Z)]
53 (.fromAngleAxis 75 (.fromAngleAxis
54 (.angleBetween X x) 76 (.angleBetween X x)
55 (let [cross (.normalize (.cross X x))] 77 (let [cross (.normalize (.cross X x))]
56 (if (= 0 (.length cross)) y cross)))) 78 (if (= 0 (.length cross)) y cross))))
57 roll (absolute-angle (.mult reverse Y) y x)] 79 roll (absolute-angle (.mult reverse Y) y x)]
58
59 [heading pitch roll])))) 80 [heading pitch roll]))))
60 81
61 (defn proprioception 82 (defn proprioception!
62 "Create a function that provides proprioceptive information about an 83 "Endow the creature with the sense of proprioception. Returns a
63 entire body." 84 sequence of functions, one for each child of the \"joints\" node in
85 the creature, which each report proprioceptive information about
86 that joint."
64 [#^Node creature] 87 [#^Node creature]
65 ;; extract the body's joints 88 ;; extract the body's joints
66 (let [joints (creature-joints creature) 89 (let [senses (map (partial proprioception-fn creature)
67 senses (map (partial joint-proprioception creature) joints)] 90 (joints creature))]
68 (fn [] 91 (fn []
69 (map #(%) senses)))) 92 (map #(%) senses))))
70 93
71 94
72 95