1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/org/touch.org	Sat Feb 04 06:54:14 2012 -0700
     1.3 @@ -0,0 +1,488 @@
     1.4 +#+title: Simulated Sense of Touch
     1.5 +#+author: Robert McIntyre
     1.6 +#+email: rlm@mit.edu
     1.7 +#+description: Simulated touch for AI research using JMonkeyEngine and clojure.
     1.8 +#+keywords: simulation, tactile sense, jMonkeyEngine3, clojure
     1.9 +#+SETUPFILE: ../../aurellem/org/setup.org
    1.10 +#+INCLUDE: ../../aurellem/org/level-0.org
    1.11 +
    1.12 +
    1.13 +* Touch
    1.14 +
    1.15 +My creatures need to be able to feel their environments. The idea here
    1.16 +is to create thousands of small /touch receptors/ along the geometries
    1.17 +which make up the creature's body. The number of touch receptors in a
    1.18 +given area is determined by how complicated that area is, as
    1.19 +determined by the total number of triangles in that region. This way,
    1.20 +complicated regions like the hands/face, etc. get more touch receptors
    1.21 +than simpler areas of the body.
    1.22 +
    1.23 +#+name: skin-main
    1.24 +#+begin_src clojure
    1.25 +(ns cortex.touch
    1.26 +  "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
    1.27 +  to be outfitted with touch sensors with density proportional to the
    1.28 +  density of triangles along the surface of the Geometry. Enables a
    1.29 +  Geometry to know what parts of itself are touching nearby objects."
    1.30 +  {:author "Robert McIntyre"}
    1.31 +  (:use (cortex world util sense))
    1.32 +  (:import com.jme3.scene.Geometry)
    1.33 +  (:import com.jme3.collision.CollisionResults)
    1.34 +  (:import jme3tools.converters.ImageToAwt)
    1.35 +  (:import (com.jme3.math Triangle Vector3f Ray)))
    1.36 +   
    1.37 +(use 'clojure.contrib.def)
    1.38 +(cortex.import/mega-import-jme3)
    1.39 +
    1.40 +(defn triangles
    1.41 +  "Return a sequence of all the Triangles which compose a given
    1.42 +  Geometry." 
    1.43 +  [#^Geometry geom]
    1.44 +  (let
    1.45 +      [mesh (.getMesh geom)
    1.46 +       triangles (transient [])]
    1.47 +    (dorun
    1.48 +     (for [n (range (.getTriangleCount mesh))]
    1.49 +       (let [tri (Triangle.)]
    1.50 +         (.getTriangle mesh n tri)
    1.51 +        ;; (.calculateNormal tri)
    1.52 +        ;; (.calculateCenter tri)
    1.53 +         (conj! triangles tri))))
    1.54 +    (persistent! triangles)))
    1.55 +   
    1.56 +(defn get-ray-origin
    1.57 +  "Return the origin which a Ray would have to have to be in the exact
    1.58 +  center of a particular Triangle in the Geometry in World
    1.59 +  Coordinates."
    1.60 +  [geom tri]
    1.61 +  (let [new (Vector3f.)]
    1.62 +    (.calculateCenter tri)
    1.63 +    (.localToWorld geom (.getCenter tri) new) new))
    1.64 +
    1.65 +(defn get-ray-direction
    1.66 +  "Return the direction which a Ray would have to have to be to point
    1.67 +  normal to the Triangle, in coordinates relative to the center of the
    1.68 +  Triangle."
    1.69 +  [geom tri]
    1.70 +  (let [n+c (Vector3f.)]
    1.71 +    (.calculateNormal tri)
    1.72 +    (.calculateCenter tri)
    1.73 +    (.localToWorld
    1.74 +     geom
    1.75 +     (.add (.getCenter tri) (.getNormal tri)) n+c)
    1.76 +    (.subtract n+c (get-ray-origin geom tri))))
    1.77 +
    1.78 +;; Every Mesh has many triangles, each with its own index.
    1.79 +;; Every vertex has its own index as well.
    1.80 +
    1.81 +(defn tactile-sensor-image
    1.82 +  "Return the touch-sensor distribution image in BufferedImage format,
    1.83 +   or nil if it does not exist."
    1.84 +  [#^Geometry obj]
    1.85 +  (if-let [image-path (meta-data obj "touch")]
    1.86 +    (ImageToAwt/convert
    1.87 +     (.getImage
    1.88 +      (.loadTexture
    1.89 +       (asset-manager)
    1.90 +       image-path))
    1.91 +    false false 0)))
    1.92 +     
    1.93 +
    1.94 +
    1.95 +(defn triangle
    1.96 +  "Get the triangle specified by triangle-index from the mesh within
    1.97 +  bounds."
    1.98 +  [#^Mesh mesh triangle-index]
    1.99 +  (let [scratch (Triangle.)]
   1.100 +    (.getTriangle mesh triangle-index scratch)
   1.101 +    scratch))
   1.102 +
   1.103 +(defn triangle-vertex-indices
   1.104 +  "Get the triangle vertex indices of a given triangle from a given
   1.105 +   mesh."
   1.106 +  [#^Mesh mesh triangle-index]
   1.107 +  (let [indices (int-array 3)]
   1.108 +    (.getTriangle mesh triangle-index indices)
   1.109 +    (vec indices)))
   1.110 +
   1.111 +(defn vertex-UV-coord
   1.112 +  "Get the uv-coordinates of the vertex named by vertex-index"
   1.113 +  [#^Mesh mesh vertex-index]
   1.114 +  (let [UV-buffer
   1.115 +        (.getData
   1.116 +         (.getBuffer
   1.117 +          mesh
   1.118 +          VertexBuffer$Type/TexCoord))]
   1.119 +    [(.get UV-buffer (* vertex-index 2))
   1.120 +     (.get UV-buffer (+ 1 (* vertex-index 2)))]))
   1.121 +
   1.122 +(defn triangle-UV-coord
   1.123 +  "Get the uv-cooridnates of the triangle's verticies."
   1.124 +  [#^Mesh mesh width height triangle-index]
   1.125 +  (map (fn [[u v]] (vector (* width u) (* height v)))
   1.126 +       (map (partial vertex-UV-coord mesh)
   1.127 +            (triangle-vertex-indices mesh triangle-index))))
   1.128 +  
   1.129 +(defn same-side?
   1.130 +  "Given the points p1 and p2 and the reference point ref, is point p
   1.131 +  on the same side of the line that goes through p1 and p2 as ref is?" 
   1.132 +  [p1 p2 ref p]
   1.133 +  (<=
   1.134 +   0
   1.135 +   (.dot 
   1.136 +    (.cross (.subtract p2 p1) (.subtract p p1))
   1.137 +    (.cross (.subtract p2 p1) (.subtract ref p1)))))
   1.138 +
   1.139 +(defn triangle-seq [#^Triangle tri]
   1.140 +  [(.get1 tri) (.get2 tri) (.get3 tri)])
   1.141 +
   1.142 +(defn vector3f-seq [#^Vector3f v]
   1.143 +  [(.getX v) (.getY v) (.getZ v)])
   1.144 +
   1.145 +(defn inside-triangle?
   1.146 +  "Is the point inside the triangle?"
   1.147 +  {:author "Dylan Holmes"}
   1.148 +  [#^Triangle tri #^Vector3f p]
   1.149 +  (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
   1.150 +    (and
   1.151 +     (same-side? vert-1 vert-2 vert-3 p)
   1.152 +     (same-side? vert-2 vert-3 vert-1 p)
   1.153 +     (same-side? vert-3 vert-1 vert-2 p))))
   1.154 +
   1.155 +(defn triangle->matrix4f
   1.156 +  "Converts the triangle into a 4x4 matrix: The first three columns
   1.157 +   contain the vertices of the triangle; the last contains the unit
   1.158 +   normal of the triangle. The bottom row is filled with 1s."
   1.159 +  [#^Triangle t]
   1.160 +  (let [mat (Matrix4f.)
   1.161 +        [vert-1 vert-2 vert-3]
   1.162 +        ((comp vec map) #(.get t %) (range 3))
   1.163 +        unit-normal (do (.calculateNormal t)(.getNormal t))
   1.164 +        vertices [vert-1 vert-2 vert-3 unit-normal]]
   1.165 +    (dorun 
   1.166 +     (for [row (range 4) col (range 3)]
   1.167 +       (do
   1.168 +         (.set mat col row (.get (vertices row)col))
   1.169 +         (.set mat 3 row 1))))
   1.170 +    mat))
   1.171 +
   1.172 +(defn triangle-transformation
   1.173 +  "Returns the affine transformation that converts each vertex in the
   1.174 +   first triangle into the corresponding vertex in the second
   1.175 +   triangle."
   1.176 +  [#^Triangle tri-1 #^Triangle tri-2]
   1.177 +  (.mult 
   1.178 +   (triangle->matrix4f tri-2)
   1.179 +   (.invert (triangle->matrix4f tri-1))))
   1.180 +
   1.181 +(defn point->vector2f [[u v]]
   1.182 +  (Vector2f. u v))
   1.183 +
   1.184 +(defn vector2f->vector3f [v]
   1.185 +  (Vector3f. (.getX v) (.getY v) 0))
   1.186 +
   1.187 +(defn map-triangle [f #^Triangle tri]
   1.188 +  (Triangle.
   1.189 +   (f 0 (.get1 tri))
   1.190 +   (f 1 (.get2 tri))
   1.191 +   (f 2 (.get3 tri))))
   1.192 +
   1.193 +(defn points->triangle
   1.194 +  "Convert a list of points into a triangle."
   1.195 +  [points]
   1.196 +  (apply #(Triangle. %1 %2 %3)
   1.197 +         (map (fn [point]
   1.198 +                (let [point (vec point)]
   1.199 +                  (Vector3f. (get point 0 0)
   1.200 +                             (get point 1 0)
   1.201 +                             (get point 2 0))))
   1.202 +              (take 3 points))))
   1.203 +
   1.204 +(defn convex-bounds
   1.205 +  ;;dylan
   1.206 +  "Returns the smallest square containing the given
   1.207 +vertices, as a vector of integers [left top width height]."
   1.208 + ;; "Dimensions of the smallest integer bounding square of the list of
   1.209 + ;;  2D verticies in the form: [x y width height]."
   1.210 +  [uv-verts]
   1.211 +  (let [xs (map first uv-verts)
   1.212 +        ys (map second uv-verts)
   1.213 +        x0 (Math/floor (apply min xs))
   1.214 +        y0 (Math/floor (apply min ys))
   1.215 +        x1 (Math/ceil (apply max xs))
   1.216 +        y1 (Math/ceil (apply max ys))]
   1.217 +    [x0 y0 (- x1 x0) (- y1 y0)]))
   1.218 +
   1.219 +(defn sensors-in-triangle
   1.220 +  ;;dylan
   1.221 +  "Locate the touch sensors in the triangle, returning a map of their UV and geometry-relative coordinates."
   1.222 +  ;;"Find the locations of the touch sensors within a triangle in both
   1.223 +  ;; UV and gemoetry relative coordinates."
   1.224 +  [image mesh tri-index]
   1.225 +  (let [width (.getWidth image)
   1.226 +        height (.getHeight image)
   1.227 +        UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
   1.228 +        bounds (convex-bounds UV-vertex-coords)
   1.229 +        
   1.230 +        cutout-triangle (points->triangle UV-vertex-coords)
   1.231 +        UV-sensor-coords
   1.232 +        (filter (comp (partial inside-triangle? cutout-triangle)
   1.233 +                      (fn [[u v]] (Vector3f. u v 0)))
   1.234 +                (white-coordinates image bounds))
   1.235 +        UV->geometry (triangle-transformation
   1.236 +                      cutout-triangle
   1.237 +                      (triangle mesh tri-index))
   1.238 +        geometry-sensor-coords
   1.239 +        (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
   1.240 +             UV-sensor-coords)]
   1.241 +  {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
   1.242 +
   1.243 +(defn-memo locate-feelers
   1.244 +  "Search the geometry's tactile UV image for touch sensors, returning
   1.245 +  their positions in geometry-relative coordinates."
   1.246 +  [#^Geometry geo]
   1.247 +  (let [mesh (.getMesh geo)
   1.248 +        num-triangles (.getTriangleCount mesh)]
   1.249 +    (if-let [image (tactile-sensor-image geo)]
   1.250 +      (map
   1.251 +       (partial sensors-in-triangle image mesh)
   1.252 +       (range num-triangles))
   1.253 +      (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
   1.254 +
   1.255 +(defn-memo touch-topology [#^Gemoetry geo]
   1.256 +  (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
   1.257 +
   1.258 +(defn-memo feeler-coordinates [#^Geometry geo]
   1.259 +  (vec (map :geometry (locate-feelers geo))))
   1.260 +
   1.261 +(defn enable-touch [#^Geometry geo]
   1.262 +  (let [feeler-coords (feeler-coordinates geo)
   1.263 +        tris (triangles geo)
   1.264 +        limit 0.1
   1.265 +        ;;results (CollisionResults.)
   1.266 +        ]
   1.267 +    (if (empty? (touch-topology geo))
   1.268 +      nil
   1.269 +      (fn [node]
   1.270 +        (let [sensor-origins 
   1.271 +              (map
   1.272 +               #(map (partial local-to-world geo) %)
   1.273 +               feeler-coords)
   1.274 +              triangle-normals 
   1.275 +              (map (partial get-ray-direction geo)
   1.276 +                   tris)
   1.277 +              rays
   1.278 +              (flatten
   1.279 +               (map (fn [origins norm]
   1.280 +                      (map #(doto (Ray. % norm)
   1.281 +                              (.setLimit limit)) origins))
   1.282 +                    sensor-origins triangle-normals))]
   1.283 +          (vector
   1.284 +           (touch-topology geo)
   1.285 +           (vec
   1.286 +            (for [ray rays]
   1.287 +              (do
   1.288 +                (let [results (CollisionResults.)]
   1.289 +                  (.collideWith node ray results)
   1.290 +                  (let [touch-objects
   1.291 +                        (filter #(not (= geo (.getGeometry %)))
   1.292 +                                results)]
   1.293 +                    (- 255
   1.294 +                       (if (empty? touch-objects) 255
   1.295 +                           (rem 
   1.296 +                            (int
   1.297 +                             (* 255 (/ (.getDistance
   1.298 +                                        (first touch-objects)) limit)))
   1.299 +                            256))))))))))))))
   1.300 +                         
   1.301 +  
   1.302 +(defn touch [#^Node pieces]
   1.303 +  (filter (comp not nil?)
   1.304 +          (map enable-touch
   1.305 +               (filter #(isa? (class %) Geometry)
   1.306 +                       (node-seq pieces)))))
   1.307 +
   1.308 +
   1.309 +#+end_src
   1.310 +
   1.311 +
   1.312 +* Example
   1.313 +
   1.314 +#+name: touch-test
   1.315 +#+begin_src clojure 
   1.316 +(ns cortex.test.touch
   1.317 +  (:use (cortex world util touch))
   1.318 +  (:import
   1.319 +   com.jme3.scene.shape.Sphere
   1.320 +   com.jme3.math.ColorRGBA
   1.321 +   com.jme3.math.Vector3f
   1.322 +   com.jme3.material.RenderState$BlendMode
   1.323 +   com.jme3.renderer.queue.RenderQueue$Bucket
   1.324 +   com.jme3.scene.shape.Box
   1.325 +   com.jme3.scene.Node))
   1.326 +
   1.327 +(defn ray-origin-debug
   1.328 +  [ray color]
   1.329 +  (make-shape
   1.330 +   (assoc base-shape
   1.331 +     :shape (Sphere. 5 5 0.05)
   1.332 +     :name "arrow"
   1.333 +     :color color
   1.334 +     :texture false
   1.335 +     :physical? false
   1.336 +     :position
   1.337 +     (.getOrigin ray))))
   1.338 +
   1.339 +(defn ray-debug [ray color]
   1.340 +  (make-shape
   1.341 +   (assoc
   1.342 +       base-shape
   1.343 +     :name "debug-ray"
   1.344 +     :physical? false
   1.345 +     :shape (com.jme3.scene.shape.Line.
   1.346 +             (.getOrigin ray)
   1.347 +             (.add
   1.348 +              (.getOrigin ray)
   1.349 +              (.mult (.getDirection ray)
   1.350 +                     (float (.getLimit ray))))))))
   1.351 +             
   1.352 +
   1.353 +(defn contact-color [contacts]
   1.354 +  (case contacts
   1.355 +    0 ColorRGBA/Gray
   1.356 +    1  ColorRGBA/Red 
   1.357 +    2  ColorRGBA/Green 
   1.358 +    3  ColorRGBA/Yellow 
   1.359 +    4  ColorRGBA/Orange 
   1.360 +    5  ColorRGBA/Red 
   1.361 +    6  ColorRGBA/Magenta 
   1.362 +    7  ColorRGBA/Pink 
   1.363 +    8  ColorRGBA/White))
   1.364 +
   1.365 +(defn update-ray-debug [node ray contacts]
   1.366 +  (let [origin (.getChild node 0)]
   1.367 +    (.setLocalTranslation origin (.getOrigin ray))
   1.368 +    (.setColor (.getMaterial origin) "Color" (contact-color contacts))))
   1.369 +
   1.370 +(defn init-node
   1.371 +  [debug-node rays]
   1.372 +    (.detachAllChildren debug-node)
   1.373 +    (dorun 
   1.374 +     (for [ray rays]
   1.375 +       (do
   1.376 +         (.attachChild
   1.377 +          debug-node 
   1.378 +          (doto (Node.)
   1.379 +            (.attachChild (ray-origin-debug ray ColorRGBA/Gray))
   1.380 +            (.attachChild (ray-debug ray ColorRGBA/Gray))
   1.381 +            ))))))
   1.382 +
   1.383 +(defn manage-ray-debug-node [debug-node geom touch-data limit]
   1.384 +  (let [rays (normal-rays limit geom)]
   1.385 +    (if (not= (count (.getChildren debug-node)) (count touch-data))
   1.386 +      (init-node debug-node rays))
   1.387 +    (dorun 
   1.388 +     (for [n (range (count touch-data))]
   1.389 +       (update-ray-debug
   1.390 +        (.getChild debug-node n) (nth rays n) (nth touch-data n))))))
   1.391 +
   1.392 +(defn transparent-sphere []
   1.393 +  (doto
   1.394 +      (make-shape
   1.395 +       (merge base-shape
   1.396 +	      {:position (Vector3f. 0 2 0)
   1.397 +	       :name "the blob."
   1.398 +	       :material "Common/MatDefs/Misc/Unshaded.j3md"
   1.399 +	       :texture "Textures/purpleWisp.png"
   1.400 +	       :physical? true
   1.401 +	       :mass 70
   1.402 +	       :color ColorRGBA/Blue
   1.403 +	       :shape (Sphere. 10 10 1)}))
   1.404 +    (-> (.getMaterial)
   1.405 +	(.getAdditionalRenderState)
   1.406 +	(.setBlendMode RenderState$BlendMode/Alpha))
   1.407 +    (.setQueueBucket RenderQueue$Bucket/Transparent)))
   1.408 +
   1.409 +(defn transparent-box []
   1.410 +    (doto
   1.411 +      (make-shape
   1.412 +       (merge base-shape
   1.413 +	      {:position (Vector3f. 0 2 0)
   1.414 +	       :name "box"
   1.415 +	       :material "Common/MatDefs/Misc/Unshaded.j3md"
   1.416 +	       :texture "Textures/purpleWisp.png"
   1.417 +	       :physical? true
   1.418 +	       :mass 70
   1.419 +	       :color ColorRGBA/Blue
   1.420 +	       :shape (Box. 1 1 1)}))
   1.421 +    (-> (.getMaterial)
   1.422 +	(.getAdditionalRenderState)
   1.423 +	(.setBlendMode RenderState$BlendMode/Alpha))
   1.424 +    (.setQueueBucket RenderQueue$Bucket/Transparent)))
   1.425 +
   1.426 +(defn transparent-floor []
   1.427 +  (doto
   1.428 +      (box 5 0.2 5  :mass 0 :position (Vector3f. 0 -2 0)
   1.429 +           :material "Common/MatDefs/Misc/Unshaded.j3md"
   1.430 +           :texture "Textures/redWisp.png"
   1.431 +           :name "floor")
   1.432 +    (-> (.getMaterial)
   1.433 +        (.getAdditionalRenderState)
   1.434 +        (.setBlendMode RenderState$BlendMode/Alpha))
   1.435 +    (.setQueueBucket RenderQueue$Bucket/Transparent)))
   1.436 +
   1.437 +(defn test-skin 
   1.438 +  "Testing touch:
   1.439 +   you should see a ball which responds to the table
   1.440 +   and whatever balls hit it."
   1.441 +  []
   1.442 +  (let [b
   1.443 +        ;;(transparent-box)
   1.444 +        (transparent-sphere)
   1.445 +        ;;(sphere)
   1.446 +        f (transparent-floor)
   1.447 +        debug-node (Node.)
   1.448 +        node      (doto (Node.) (.attachChild b) (.attachChild f))
   1.449 +        root-node (doto (Node.) (.attachChild node)
   1.450 +                        (.attachChild debug-node))
   1.451 +        ]
   1.452 +    
   1.453 +    (world
   1.454 +     root-node
   1.455 +     {"key-return" (fire-cannon-ball node)}
   1.456 +     (fn [world]
   1.457 +       ;;  (Capture/SimpleCaptureVideo
   1.458 +       ;;   world 
   1.459 +       ;;   (file-str "/home/r/proj/cortex/tmp/blob.avi"))
   1.460 +       ;;  (no-logging)
   1.461 +       ;;(enable-debug world)
   1.462 +       ;;  (set-accuracy world (/ 1 60))
   1.463 +       )
   1.464 +     
   1.465 +     (fn [& _]
   1.466 +       (let [sensitivity 0.2
   1.467 +             touch-data (touch-percieve sensitivity b node)]
   1.468 +         (manage-ray-debug-node debug-node b touch-data sensitivity))
   1.469 +       ))))
   1.470 +
   1.471 +
   1.472 +#+end_src
   1.473 +
   1.474 +
   1.475 +
   1.476 +
   1.477 +  
   1.478 +* COMMENT code generation
   1.479 +#+begin_src clojure :tangle ../src/cortex/touch.clj
   1.480 +<<skin-main>>
   1.481 +#+end_src
   1.482 +
   1.483 +#+begin_src clojure :tangle ../src/cortex/test/touch.clj 
   1.484 +<<touch-test>>
   1.485 +#+end_src
   1.486 +
   1.487 +
   1.488 +
   1.489 +  
   1.490 +
   1.491 +