view org/proprioception.org @ 189:facc2ef3fe5c

added hearing debug view.
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 10:13:15 -0700
parents 0b9ae09eaec3
children 2902aca33c6e
line wrap: on
line source
1 #+title: The Sense of Proprioception
2 #+author: Robert McIntyre
3 #+email: rlm@mit.edu
4 #+description: proprioception for simulated creatures
5 #+keywords: simulation, jMonkeyEngine3, clojure
6 #+SETUPFILE: ../../aurellem/org/setup.org
7 #+INCLUDE: ../../aurellem/org/level-0.org
9 #+name: proprioception
10 #+begin_src clojure
11 (ns cortex.proprioception
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 (:import com.jme3.scene.Node)
19 (:import (com.jme3.math Vector3f Quaternion)))
21 (defn right-handed?
22 "true iff the three vectors form a right handed coordinate
23 system. The three vectors do not have to be normalized or
24 orthogonal."
25 [vec1 vec2 vec3]
26 (< 0 (.dot (.cross vec1 vec2) vec3)))
28 (defn absolute-angle
29 "The angle between 'vec1 and 'vec2. Positive if the angle to get
30 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
31 otherwise."
32 [vec1 vec2 axis]
33 (let [angle (.angleBetween vec1 vec2)]
34 (if (right-handed? vec1 vec2 axis)
35 angle (- (* 2 Math/PI) angle))))
37 (defn proprioception-fn
38 "Returns a function which returns proprioceptive sensory data when
39 called inside a running simulation."
40 [#^Node parts #^Node joint]
41 (let [[obj-a obj-b] (joint-targets parts joint)
42 joint-rot (.getWorldRotation joint)
43 x0 (.mult joint-rot Vector3f/UNIT_X)
44 y0 (.mult joint-rot Vector3f/UNIT_Y)
45 z0 (.mult joint-rot Vector3f/UNIT_Z)]
46 (println-repl "x:" x0)
47 (println-repl "y:" y0)
48 (println-repl "z:" z0)
49 (println-repl "init-a:" (.getWorldRotation obj-a))
50 (println-repl "init-b:" (.getWorldRotation obj-b))
52 (fn []
53 (let [rot-a (.clone (.getWorldRotation obj-a))
54 rot-b (.clone (.getWorldRotation obj-b))
55 x (.mult rot-a x0)
56 y (.mult rot-a y0)
57 z (.mult rot-a z0)
59 X (.mult rot-b x0)
60 Y (.mult rot-b y0)
61 Z (.mult rot-b z0)
62 heading (Math/atan2 (.dot X z) (.dot X x))
63 pitch (Math/atan2 (.dot X y) (.dot X x))
65 ;; rotate x-vector back to origin
66 reverse
67 (doto (Quaternion.)
68 (.fromAngleAxis
69 (.angleBetween X x)
70 (let [cross (.normalize (.cross X x))]
71 (if (= 0 (.length cross)) y cross))))
72 roll (absolute-angle (.mult reverse Y) y x)]
73 [heading pitch roll]))))
75 (defn proprioception!
76 "Endow the creature with the sense of proprioception. Returns a
77 sequence of functions, one for each child of the \"joints\" node in
78 the creature, which each report proprioceptive information about
79 that joint."
80 [#^Node creature]
81 ;; extract the body's joints
82 (let [senses (map (partial proprioception-fn creature)
83 (joints creature))]
84 (fn []
85 (map #(%) senses))))
88 (import java.awt.image.BufferedImage)
90 (defn draw-sprite [image sprite x y color ]
91 (dorun
92 (for [[u v] sprite]
93 (.setRGB image (+ u x) (+ v y) color))))
95 (defn view-angle
96 "create a debug view of an angle"
97 [color]
98 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
99 previous (atom [25 25])
100 sprite [[0 0] [0 1]
101 [0 -1] [-1 0] [1 0]]]
102 (fn [angle]
103 (let [angle (float angle)]
104 (let [position
105 [(+ 25 (int (* 20 (Math/cos angle))))
106 (+ 25 (int (* -20 (Math/sin angle))))]]
107 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
108 (draw-sprite image sprite (position 0) (position 1) color)
109 (reset! previous position))
110 image))))
112 (defn proprioception-debug-window
113 []
114 (let [heading (view-angle 0xFF0000)
115 pitch (view-angle 0x00FF00)
116 roll (view-angle 0xFFFFFF)
117 v-heading (view-image)
118 v-pitch (view-image)
119 v-roll (view-image)
120 ]
121 (fn [prop-data]
122 (dorun
123 (map
124 (fn [[h p r]]
125 (v-heading (heading h))
126 (v-pitch (pitch p))
127 (v-roll (roll r)))
128 prop-data)))))
130 #+end_src
132 * COMMENT generate source
133 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
134 <<proprioception>>
135 #+end_src