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