view org/skin.org @ 158:811127d79d24

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