view org/touch.org @ 227:2a7f57e7efdb

working on touch.org
author Robert McIntyre <rlm@mit.edu>
date Sat, 11 Feb 2012 14:12:17 -0700
parents e5db1d2ff9a8
children 0589c35f04f2
line wrap: on
line source
1 #+title: Simulated Sense of Touch
2 #+author: Robert McIntyre
3 #+email: rlm@mit.edu
4 #+description: Simulated touch for AI research using JMonkeyEngine and clojure.
5 #+keywords: simulation, tactile sense, jMonkeyEngine3, clojure
6 #+SETUPFILE: ../../aurellem/org/setup.org
7 #+INCLUDE: ../../aurellem/org/level-0.org
9 * Touch
11 Touch is critical to navigation and spatial reasoning and as such I
12 need a simulated version of it to give to my AI creatures.
14 #+name: skin-main
15 #+begin_src clojure
16 (in-ns 'cortex.touch)
18 (defn triangles
19 "Return a sequence of all the Triangles which compose a given
20 Geometry."
21 [#^Geometry geom]
22 (let
23 [mesh (.getMesh geom)
24 triangles (transient [])]
25 (dorun
26 (for [n (range (.getTriangleCount mesh))]
27 (let [tri (Triangle.)]
28 (.getTriangle mesh n tri)
29 ;; (.calculateNormal tri)
30 ;; (.calculateCenter tri)
31 (conj! triangles tri))))
32 (persistent! triangles)))
34 (defn get-ray-origin
35 "Return the origin which a Ray would have to have to be in the exact
36 center of a particular Triangle in the Geometry in World
37 Coordinates."
38 [geom tri]
39 (let [new (Vector3f.)]
40 (.calculateCenter tri)
41 (.localToWorld geom (.getCenter tri) new) new))
43 (defn get-ray-direction
44 "Return the direction which a Ray would have to have to be to point
45 normal to the Triangle, in coordinates relative to the center of the
46 Triangle."
47 [geom tri]
48 (let [n+c (Vector3f.)]
49 (.calculateNormal tri)
50 (.calculateCenter tri)
51 (.localToWorld
52 geom
53 (.add (.getCenter tri) (.getNormal tri)) n+c)
54 (.subtract n+c (get-ray-origin geom tri))))
56 ;; Every Mesh has many triangles, each with its own index.
57 ;; Every vertex has its own index as well.
59 (defn tactile-sensor-profile
60 "Return the touch-sensor distribution image in BufferedImage format,
61 or nil if it does not exist."
62 [#^Geometry obj]
63 (if-let [image-path (meta-data obj "touch")]
64 (load-image image-path)))
66 (defn mesh-triangle
67 "Get the triangle specified by triangle-index from the mesh within
68 bounds."
69 [#^Mesh mesh triangle-index]
70 (let [scratch (Triangle.)]
71 (.getTriangle mesh triangle-index scratch)
72 scratch))
74 (defn triangle-vertex-indices
75 "Get the triangle vertex indices of a given triangle from a given
76 mesh."
77 [#^Mesh mesh triangle-index]
78 (let [indices (int-array 3)]
79 (.getTriangle mesh triangle-index indices)
80 (vec indices)))
82 (defn vertex-UV-coord
83 "Get the UV-coordinates of the vertex named by vertex-index"
84 [#^Mesh mesh vertex-index]
85 (let [UV-buffer
86 (.getData
87 (.getBuffer
88 mesh
89 VertexBuffer$Type/TexCoord))]
90 [(.get UV-buffer (* vertex-index 2))
91 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
93 (defn triangle-UV-coord
94 "Get the UV-cooridnates of the triangle's verticies."
95 [#^Mesh mesh width height triangle-index]
96 (map (fn [[u v]] (vector (* width u) (* height v)))
97 (map (partial vertex-UV-coord mesh)
98 (triangle-vertex-indices mesh triangle-index))))
100 (defn same-side?
101 "Given the points p1 and p2 and the reference point ref, is point p
102 on the same side of the line that goes through p1 and p2 as ref is?"
103 [p1 p2 ref p]
104 (<=
105 0
106 (.dot
107 (.cross (.subtract p2 p1) (.subtract p p1))
108 (.cross (.subtract p2 p1) (.subtract ref p1)))))
110 (defn triangle-seq [#^Triangle tri]
111 [(.get1 tri) (.get2 tri) (.get3 tri)])
113 (defn vector3f-seq [#^Vector3f v]
114 [(.getX v) (.getY v) (.getZ v)])
116 (defn inside-triangle?
117 "Is the point inside the triangle?"
118 {:author "Dylan Holmes"}
119 [#^Triangle tri #^Vector3f p]
120 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
121 (and
122 (same-side? vert-1 vert-2 vert-3 p)
123 (same-side? vert-2 vert-3 vert-1 p)
124 (same-side? vert-3 vert-1 vert-2 p))))
126 (defn triangle->matrix4f
127 "Converts the triangle into a 4x4 matrix: The first three columns
128 contain the vertices of the triangle; the last contains the unit
129 normal of the triangle. The bottom row is filled with 1s."
130 [#^Triangle t]
131 (let [mat (Matrix4f.)
132 [vert-1 vert-2 vert-3]
133 ((comp vec map) #(.get t %) (range 3))
134 unit-normal (do (.calculateNormal t)(.getNormal t))
135 vertices [vert-1 vert-2 vert-3 unit-normal]]
136 (dorun
137 (for [row (range 4) col (range 3)]
138 (do
139 (.set mat col row (.get (vertices row)col))
140 (.set mat 3 row 1))))
141 mat))
143 (defn triangle-transformation
144 "Returns the affine transformation that converts each vertex in the
145 first triangle into the corresponding vertex in the second
146 triangle."
147 [#^Triangle tri-1 #^Triangle tri-2]
148 (.mult
149 (triangle->matrix4f tri-2)
150 (.invert (triangle->matrix4f tri-1))))
152 (defn point->vector2f [[u v]]
153 (Vector2f. u v))
155 (defn vector2f->vector3f [v]
156 (Vector3f. (.getX v) (.getY v) 0))
158 (defn map-triangle [f #^Triangle tri]
159 (Triangle.
160 (f 0 (.get1 tri))
161 (f 1 (.get2 tri))
162 (f 2 (.get3 tri))))
164 (defn points->triangle
165 "Convert a list of points into a triangle."
166 [points]
167 (apply #(Triangle. %1 %2 %3)
168 (map (fn [point]
169 (let [point (vec point)]
170 (Vector3f. (get point 0 0)
171 (get point 1 0)
172 (get point 2 0))))
173 (take 3 points))))
175 (defn convex-bounds
176 "Returns the smallest square containing the given vertices, as a
177 vector of integers [left top width height]."
178 [uv-verts]
179 (let [xs (map first uv-verts)
180 ys (map second uv-verts)
181 x0 (Math/floor (apply min xs))
182 y0 (Math/floor (apply min ys))
183 x1 (Math/ceil (apply max xs))
184 y1 (Math/ceil (apply max ys))]
185 [x0 y0 (- x1 x0) (- y1 y0)]))
187 (defn sensors-in-triangle
188 "Locate the touch sensors in the triangle, returning a map of their
189 UV and geometry-relative coordinates."
190 [image mesh tri-index]
191 (let [width (.getWidth image)
192 height (.getHeight image)
193 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
194 bounds (convex-bounds UV-vertex-coords)
196 cutout-triangle (points->triangle UV-vertex-coords)
197 UV-sensor-coords
198 (filter (comp (partial inside-triangle? cutout-triangle)
199 (fn [[u v]] (Vector3f. u v 0)))
200 (white-coordinates image bounds))
201 UV->geometry (triangle-transformation
202 cutout-triangle
203 (mesh-triangle mesh tri-index))
204 geometry-sensor-coords
205 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
206 UV-sensor-coords)]
207 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
209 (defn-memo locate-feelers
210 "Search the geometry's tactile UV profile for touch sensors,
211 returning their positions in geometry-relative coordinates."
212 [#^Geometry geo]
213 (let [mesh (.getMesh geo)
214 num-triangles (.getTriangleCount mesh)]
215 (if-let [image (tactile-sensor-profile geo)]
216 (map
217 (partial sensors-in-triangle image mesh)
218 (range num-triangles))
219 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
221 (defn-memo touch-topology
222 "Return a sequence of vectors of the form [x y] describing the
223 \"topology\" of the tactile sensors. Points that are close together
224 in the touch-topology are generally close together in the simulation."
225 [#^Gemoetry geo]
226 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
228 (defn-memo feeler-coordinates
229 "The location of the touch sensors in world-space coordinates."
230 [#^Geometry geo]
231 (vec (map :geometry (locate-feelers geo))))
233 (defn touch-fn
234 "Returns a function which returns tactile sensory data when called
235 inside a running simulation."
236 [#^Geometry geo]
237 (let [feeler-coords (feeler-coordinates geo)
238 tris (triangles geo)
239 limit 0.1
240 ;;results (CollisionResults.)
241 ]
242 (if (empty? (touch-topology geo))
243 nil
244 (fn [node]
245 (let [sensor-origins
246 (map
247 #(map (partial local-to-world geo) %)
248 feeler-coords)
249 triangle-normals
250 (map (partial get-ray-direction geo)
251 tris)
252 rays
253 (flatten
254 (map (fn [origins norm]
255 (map #(doto (Ray. % norm)
256 (.setLimit limit)) origins))
257 sensor-origins triangle-normals))]
258 (vector
259 (touch-topology geo)
260 (vec
261 (for [ray rays]
262 (do
263 (let [results (CollisionResults.)]
264 (.collideWith node ray results)
265 (let [touch-objects
266 (filter #(not (= geo (.getGeometry %)))
267 results)]
268 (- 255
269 (if (empty? touch-objects) 255
270 (rem
271 (int
272 (* 255 (/ (.getDistance
273 (first touch-objects)) limit)))
274 256))))))))))))))
276 (defn touch!
277 "Endow the creature with the sense of touch. Returns a sequence of
278 functions, one for each body part with a tactile-sensor-proile,
279 each of which when called returns sensory data for that body part."
280 [#^Node creature]
281 (filter
282 (comp not nil?)
283 (map touch-fn
284 (filter #(isa? (class %) Geometry)
285 (node-seq creature)))))
287 (defn view-touch
288 "Creates a function which accepts a list of touch sensor-data and
289 displays each element to the screen."
290 []
291 (view-sense
292 (fn
293 [[coords sensor-data]]
294 (let [image (points->image coords)]
295 (dorun
296 (for [i (range (count coords))]
297 (.setRGB image ((coords i) 0) ((coords i) 1)
298 (gray (sensor-data i)))))
299 image))))
300 #+end_src
302 * Headers
303 #+begin_src clojure
304 (ns cortex.touch
305 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
306 to be outfitted with touch sensors with density determined by a UV
307 image. In this way a Geometry can know what parts of itself are
308 touching nearby objects. Reads specially prepared blender files to
309 construct this sense automatically."
310 {:author "Robert McIntyre"}
311 (:use (cortex world util sense))
312 (:use clojure.contrib.def)
313 (:import (com.jme3.scene Geometry Node Mesh))
314 (:import com.jme3.collision.CollisionResults)
315 (:import com.jme3.scene.VertexBuffer$Type)
316 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
317 #+end_src
319 * COMMENT Code Generation
320 #+begin_src clojure :tangle ../src/cortex/touch.clj
321 <<skin-main>>
322 #+end_src
324 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
325 #+end_src