Mercurial > cortex

annotate org/touch.org @ 177:5af4ebe72b97

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