view org/touch.org @ 179:11bd5f0625ad

organized imports in touch
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 07:09:40 -0700
parents 6fba17a74a57
children cfb71209ddc6
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 My creatures need to be able to feel their environments. The idea here
12 is to create thousands of small /touch receptors/ along the geometries
13 which make up the creature's body. The number of touch receptors in a
14 given area is determined by how complicated that area is, as
15 determined by the total number of triangles in that region. This way,
16 complicated regions like the hands/face, etc. get more touch receptors
17 than simpler areas of the body.
19 #+name: skin-main
20 #+begin_src clojure
21 (ns cortex.touch
22 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
23 to be outfitted with touch sensors with density proportional to the
24 density of triangles along the surface of the Geometry. Enables a
25 Geometry to know what parts of itself are touching nearby objects."
26 {:author "Robert McIntyre"}
27 (:use (cortex world util sense))
28 (:use clojure.contrib.def)
29 (:import (com.jme3.scene Geometry Node Mesh))
30 (:import com.jme3.collision.CollisionResults)
31 (:import com.jme3.scene.VertexBuffer$Type)
32 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
34 (defn triangles
35 "Return a sequence of all the Triangles which compose a given
36 Geometry."
37 [#^Geometry geom]
38 (let
39 [mesh (.getMesh geom)
40 triangles (transient [])]
41 (dorun
42 (for [n (range (.getTriangleCount mesh))]
43 (let [tri (Triangle.)]
44 (.getTriangle mesh n tri)
45 ;; (.calculateNormal tri)
46 ;; (.calculateCenter tri)
47 (conj! triangles tri))))
48 (persistent! triangles)))
50 (defn get-ray-origin
51 "Return the origin which a Ray would have to have to be in the exact
52 center of a particular Triangle in the Geometry in World
53 Coordinates."
54 [geom tri]
55 (let [new (Vector3f.)]
56 (.calculateCenter tri)
57 (.localToWorld geom (.getCenter tri) new) new))
59 (defn get-ray-direction
60 "Return the direction which a Ray would have to have to be to point
61 normal to the Triangle, in coordinates relative to the center of the
62 Triangle."
63 [geom tri]
64 (let [n+c (Vector3f.)]
65 (.calculateNormal tri)
66 (.calculateCenter tri)
67 (.localToWorld
68 geom
69 (.add (.getCenter tri) (.getNormal tri)) n+c)
70 (.subtract n+c (get-ray-origin geom tri))))
72 ;; Every Mesh has many triangles, each with its own index.
73 ;; Every vertex has its own index as well.
75 (defn tactile-sensor-profile
76 "Return the touch-sensor distribution image in BufferedImage format,
77 or nil if it does not exist."
78 [#^Geometry obj]
79 (if-let [image-path (meta-data obj "touch")]
80 (load-image image-path)))
82 (defn triangle
83 "Get the triangle specified by triangle-index from the mesh within
84 bounds."
85 [#^Mesh mesh triangle-index]
86 (let [scratch (Triangle.)]
87 (.getTriangle mesh triangle-index scratch)
88 scratch))
90 (defn triangle-vertex-indices
91 "Get the triangle vertex indices of a given triangle from a given
92 mesh."
93 [#^Mesh mesh triangle-index]
94 (let [indices (int-array 3)]
95 (.getTriangle mesh triangle-index indices)
96 (vec indices)))
98 (defn vertex-UV-coord
99 "Get the uv-coordinates of the vertex named by vertex-index"
100 [#^Mesh mesh vertex-index]
101 (let [UV-buffer
102 (.getData
103 (.getBuffer
104 mesh
105 VertexBuffer$Type/TexCoord))]
106 [(.get UV-buffer (* vertex-index 2))
107 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
109 (defn triangle-UV-coord
110 "Get the uv-cooridnates of the triangle's verticies."
111 [#^Mesh mesh width height triangle-index]
112 (map (fn [[u v]] (vector (* width u) (* height v)))
113 (map (partial vertex-UV-coord mesh)
114 (triangle-vertex-indices mesh triangle-index))))
116 (defn same-side?
117 "Given the points p1 and p2 and the reference point ref, is point p
118 on the same side of the line that goes through p1 and p2 as ref is?"
119 [p1 p2 ref p]
120 (<=
121 0
122 (.dot
123 (.cross (.subtract p2 p1) (.subtract p p1))
124 (.cross (.subtract p2 p1) (.subtract ref p1)))))
126 (defn triangle-seq [#^Triangle tri]
127 [(.get1 tri) (.get2 tri) (.get3 tri)])
129 (defn vector3f-seq [#^Vector3f v]
130 [(.getX v) (.getY v) (.getZ v)])
132 (defn inside-triangle?
133 "Is the point inside the triangle?"
134 {:author "Dylan Holmes"}
135 [#^Triangle tri #^Vector3f p]
136 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
137 (and
138 (same-side? vert-1 vert-2 vert-3 p)
139 (same-side? vert-2 vert-3 vert-1 p)
140 (same-side? vert-3 vert-1 vert-2 p))))
142 (defn triangle->matrix4f
143 "Converts the triangle into a 4x4 matrix: The first three columns
144 contain the vertices of the triangle; the last contains the unit
145 normal of the triangle. The bottom row is filled with 1s."
146 [#^Triangle t]
147 (let [mat (Matrix4f.)
148 [vert-1 vert-2 vert-3]
149 ((comp vec map) #(.get t %) (range 3))
150 unit-normal (do (.calculateNormal t)(.getNormal t))
151 vertices [vert-1 vert-2 vert-3 unit-normal]]
152 (dorun
153 (for [row (range 4) col (range 3)]
154 (do
155 (.set mat col row (.get (vertices row)col))
156 (.set mat 3 row 1))))
157 mat))
159 (defn triangle-transformation
160 "Returns the affine transformation that converts each vertex in the
161 first triangle into the corresponding vertex in the second
162 triangle."
163 [#^Triangle tri-1 #^Triangle tri-2]
164 (.mult
165 (triangle->matrix4f tri-2)
166 (.invert (triangle->matrix4f tri-1))))
168 (defn point->vector2f [[u v]]
169 (Vector2f. u v))
171 (defn vector2f->vector3f [v]
172 (Vector3f. (.getX v) (.getY v) 0))
174 (defn map-triangle [f #^Triangle tri]
175 (Triangle.
176 (f 0 (.get1 tri))
177 (f 1 (.get2 tri))
178 (f 2 (.get3 tri))))
180 (defn points->triangle
181 "Convert a list of points into a triangle."
182 [points]
183 (apply #(Triangle. %1 %2 %3)
184 (map (fn [point]
185 (let [point (vec point)]
186 (Vector3f. (get point 0 0)
187 (get point 1 0)
188 (get point 2 0))))
189 (take 3 points))))
191 (defn convex-bounds
192 "Returns the smallest square containing the given vertices, as a
193 vector of integers [left top width height]."
194 [uv-verts]
195 (let [xs (map first uv-verts)
196 ys (map second uv-verts)
197 x0 (Math/floor (apply min xs))
198 y0 (Math/floor (apply min ys))
199 x1 (Math/ceil (apply max xs))
200 y1 (Math/ceil (apply max ys))]
201 [x0 y0 (- x1 x0) (- y1 y0)]))
203 (defn sensors-in-triangle
204 "Locate the touch sensors in the triangle, returning a map of their
205 UV and geometry-relative coordinates."
206 [image mesh tri-index]
207 (let [width (.getWidth image)
208 height (.getHeight image)
209 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
210 bounds (convex-bounds UV-vertex-coords)
212 cutout-triangle (points->triangle UV-vertex-coords)
213 UV-sensor-coords
214 (filter (comp (partial inside-triangle? cutout-triangle)
215 (fn [[u v]] (Vector3f. u v 0)))
216 (white-coordinates image bounds))
217 UV->geometry (triangle-transformation
218 cutout-triangle
219 (triangle mesh tri-index))
220 geometry-sensor-coords
221 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
222 UV-sensor-coords)]
223 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
225 (defn-memo locate-feelers
226 "Search the geometry's tactile UV profile for touch sensors,
227 returning their positions in geometry-relative coordinates."
228 [#^Geometry geo]
229 (let [mesh (.getMesh geo)
230 num-triangles (.getTriangleCount mesh)]
231 (if-let [image (tactile-sensor-profile geo)]
232 (map
233 (partial sensors-in-triangle image mesh)
234 (range num-triangles))
235 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
237 (defn-memo touch-topology
238 "Return a sequence of vectors of the form [x y] describing the
239 \"topology\" of the tactile sensors. Points that are close together
240 in the touch-topology are generally close together in the simulation."
241 [#^Gemoetry geo]
242 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
244 (defn-memo feeler-coordinates
245 "The location of the touch sensors in world-space coordinates."
246 [#^Geometry geo]
247 (vec (map :geometry (locate-feelers geo))))
249 (defn touch-fn
250 "Returns a function which returns tactile sensory data when called
251 inside a running simulation."
252 [#^Geometry geo]
253 (let [feeler-coords (feeler-coordinates geo)
254 tris (triangles geo)
255 limit 0.1
256 ;;results (CollisionResults.)
257 ]
258 (if (empty? (touch-topology geo))
259 nil
260 (fn [node]
261 (let [sensor-origins
262 (map
263 #(map (partial local-to-world geo) %)
264 feeler-coords)
265 triangle-normals
266 (map (partial get-ray-direction geo)
267 tris)
268 rays
269 (flatten
270 (map (fn [origins norm]
271 (map #(doto (Ray. % norm)
272 (.setLimit limit)) origins))
273 sensor-origins triangle-normals))]
274 (vector
275 (touch-topology geo)
276 (vec
277 (for [ray rays]
278 (do
279 (let [results (CollisionResults.)]
280 (.collideWith node ray results)
281 (let [touch-objects
282 (filter #(not (= geo (.getGeometry %)))
283 results)]
284 (- 255
285 (if (empty? touch-objects) 255
286 (rem
287 (int
288 (* 255 (/ (.getDistance
289 (first touch-objects)) limit)))
290 256))))))))))))))
292 (defn touch!
293 "Endow the creature with the sense of touch. Returns a sequence of
294 functions, one for each body part with a tactile-sensor-proile,
295 each of which when called returns sensory data for that body part."
296 [#^Node creature]
297 (filter
298 (comp not nil?)
299 (map touch-fn
300 (filter #(isa? (class %) Geometry)
301 (node-seq creature)))))
304 #+end_src
307 * Example
309 #+name: touch-test
310 #+begin_src clojure
311 (ns cortex.test.touch
312 (:use (cortex world util touch))
313 (:import
314 com.jme3.scene.shape.Sphere
315 com.jme3.math.ColorRGBA
316 com.jme3.math.Vector3f
317 com.jme3.material.RenderState$BlendMode
318 com.jme3.renderer.queue.RenderQueue$Bucket
319 com.jme3.scene.shape.Box
320 com.jme3.scene.Node))
322 (defn ray-origin-debug
323 [ray color]
324 (make-shape
325 (assoc base-shape
326 :shape (Sphere. 5 5 0.05)
327 :name "arrow"
328 :color color
329 :texture false
330 :physical? false
331 :position
332 (.getOrigin ray))))
334 (defn ray-debug [ray color]
335 (make-shape
336 (assoc
337 base-shape
338 :name "debug-ray"
339 :physical? false
340 :shape (com.jme3.scene.shape.Line.
341 (.getOrigin ray)
342 (.add
343 (.getOrigin ray)
344 (.mult (.getDirection ray)
345 (float (.getLimit ray))))))))
348 (defn contact-color [contacts]
349 (case contacts
350 0 ColorRGBA/Gray
351 1 ColorRGBA/Red
352 2 ColorRGBA/Green
353 3 ColorRGBA/Yellow
354 4 ColorRGBA/Orange
355 5 ColorRGBA/Red
356 6 ColorRGBA/Magenta
357 7 ColorRGBA/Pink
358 8 ColorRGBA/White))
360 (defn update-ray-debug [node ray contacts]
361 (let [origin (.getChild node 0)]
362 (.setLocalTranslation origin (.getOrigin ray))
363 (.setColor (.getMaterial origin) "Color" (contact-color contacts))))
365 (defn init-node
366 [debug-node rays]
367 (.detachAllChildren debug-node)
368 (dorun
369 (for [ray rays]
370 (do
371 (.attachChild
372 debug-node
373 (doto (Node.)
374 (.attachChild (ray-origin-debug ray ColorRGBA/Gray))
375 (.attachChild (ray-debug ray ColorRGBA/Gray))
376 ))))))
378 (defn manage-ray-debug-node [debug-node geom touch-data limit]
379 (let [rays (normal-rays limit geom)]
380 (if (not= (count (.getChildren debug-node)) (count touch-data))
381 (init-node debug-node rays))
382 (dorun
383 (for [n (range (count touch-data))]
384 (update-ray-debug
385 (.getChild debug-node n) (nth rays n) (nth touch-data n))))))
387 (defn transparent-sphere []
388 (doto
389 (make-shape
390 (merge base-shape
391 {:position (Vector3f. 0 2 0)
392 :name "the blob."
393 :material "Common/MatDefs/Misc/Unshaded.j3md"
394 :texture "Textures/purpleWisp.png"
395 :physical? true
396 :mass 70
397 :color ColorRGBA/Blue
398 :shape (Sphere. 10 10 1)}))
399 (-> (.getMaterial)
400 (.getAdditionalRenderState)
401 (.setBlendMode RenderState$BlendMode/Alpha))
402 (.setQueueBucket RenderQueue$Bucket/Transparent)))
404 (defn transparent-box []
405 (doto
406 (make-shape
407 (merge base-shape
408 {:position (Vector3f. 0 2 0)
409 :name "box"
410 :material "Common/MatDefs/Misc/Unshaded.j3md"
411 :texture "Textures/purpleWisp.png"
412 :physical? true
413 :mass 70
414 :color ColorRGBA/Blue
415 :shape (Box. 1 1 1)}))
416 (-> (.getMaterial)
417 (.getAdditionalRenderState)
418 (.setBlendMode RenderState$BlendMode/Alpha))
419 (.setQueueBucket RenderQueue$Bucket/Transparent)))
421 (defn transparent-floor []
422 (doto
423 (box 5 0.2 5 :mass 0 :position (Vector3f. 0 -2 0)
424 :material "Common/MatDefs/Misc/Unshaded.j3md"
425 :texture "Textures/redWisp.png"
426 :name "floor")
427 (-> (.getMaterial)
428 (.getAdditionalRenderState)
429 (.setBlendMode RenderState$BlendMode/Alpha))
430 (.setQueueBucket RenderQueue$Bucket/Transparent)))
432 (defn test-skin
433 "Testing touch:
434 you should see a ball which responds to the table
435 and whatever balls hit it."
436 []
437 (let [b
438 ;;(transparent-box)
439 (transparent-sphere)
440 ;;(sphere)
441 f (transparent-floor)
442 debug-node (Node.)
443 node (doto (Node.) (.attachChild b) (.attachChild f))
444 root-node (doto (Node.) (.attachChild node)
445 (.attachChild debug-node))
446 ]
448 (world
449 root-node
450 {"key-return" (fire-cannon-ball node)}
451 (fn [world]
452 ;; (Capture/SimpleCaptureVideo
453 ;; world
454 ;; (file-str "/home/r/proj/cortex/tmp/blob.avi"))
455 ;; (no-logging)
456 ;;(enable-debug world)
457 ;; (set-accuracy world (/ 1 60))
458 )
460 (fn [& _]
461 (let [sensitivity 0.2
462 touch-data (touch-percieve sensitivity b node)]
463 (manage-ray-debug-node debug-node b touch-data sensitivity))
464 ))))
467 #+end_src
473 * COMMENT code generation
474 #+begin_src clojure :tangle ../src/cortex/touch.clj
475 <<skin-main>>
476 #+end_src
478 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
479 <<touch-test>>
480 #+end_src