cortex: org/touch.org comparison

comparison org/touch.org @ 177:5af4ebe72b97

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

comparison

equal deleted inserted replaced

-:026f69582022
+:5af4ebe72b97
+#+title: Simulated Sense of Touch
+#+author: Robert McIntyre
+#+email: rlm@mit.edu
+#+description: Simulated touch for AI research using JMonkeyEngine and clojure.
+#+keywords: simulation, tactile sense, jMonkeyEngine3, clojure
+#+SETUPFILE: ../../aurellem/org/setup.org
+#+INCLUDE: ../../aurellem/org/level-0.org
+* Touch
+My creatures need to be able to feel their environments. The idea here
+is to create thousands of small /touch receptors/ along the geometries
+which make up the creature's body. The number of touch receptors in a
+given area is determined by how complicated that area is, as
+determined by the total number of triangles in that region. This way,
+complicated regions like the hands/face, etc. get more touch receptors
+than simpler areas of the body.
+#+name: skin-main
+#+begin_src clojure
+(ns cortex.touch
+"Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
+to be outfitted with touch sensors with density proportional to the
+density of triangles along the surface of the Geometry. Enables a
+Geometry to know what parts of itself are touching nearby objects."
+{:author "Robert McIntyre"}
+(:use (cortex world util sense))
+(:import com.jme3.scene.Geometry)
+(:import com.jme3.collision.CollisionResults)
+(:import jme3tools.converters.ImageToAwt)
+(:import (com.jme3.math Triangle Vector3f Ray)))
+(use 'clojure.contrib.def)
+(cortex.import/mega-import-jme3)
+(defn triangles
+"Return a sequence of all the Triangles which compose a given
+Geometry."
+[#^Geometry geom]
+(let
+[mesh (.getMesh geom)
+triangles (transient [])]
+(dorun
+(for [n (range (.getTriangleCount mesh))]
+(let [tri (Triangle.)]
+(.getTriangle mesh n tri)
+;; (.calculateNormal tri)
+;; (.calculateCenter tri)
+(conj! triangles tri))))
+(persistent! triangles)))
+(defn get-ray-origin
+"Return the origin which a Ray would have to have to be in the exact
+center of a particular Triangle in the Geometry in World
+Coordinates."
+[geom tri]
+(let [new (Vector3f.)]
+(.calculateCenter tri)
+(.localToWorld geom (.getCenter tri) new) new))
+(defn get-ray-direction
+"Return the direction which a Ray would have to have to be to point
+normal to the Triangle, in coordinates relative to the center of the
+Triangle."
+[geom tri]
+(let [n+c (Vector3f.)]
+(.calculateNormal tri)
+(.calculateCenter tri)
+(.localToWorld
+geom
+(.add (.getCenter tri) (.getNormal tri)) n+c)
+(.subtract n+c (get-ray-origin geom tri))))
+;; Every Mesh has many triangles, each with its own index.
+;; Every vertex has its own index as well.
+(defn tactile-sensor-image
+"Return the touch-sensor distribution image in BufferedImage format,
+or nil if it does not exist."
+[#^Geometry obj]
+(if-let [image-path (meta-data obj "touch")]
+(ImageToAwt/convert
+(.getImage
+(.loadTexture
+(asset-manager)
+image-path))
+false false 0)))
+(defn triangle
+"Get the triangle specified by triangle-index from the mesh within
+bounds."
+[#^Mesh mesh triangle-index]
+(let [scratch (Triangle.)]
+(.getTriangle mesh triangle-index scratch)
+scratch))
+(defn triangle-vertex-indices
+"Get the triangle vertex indices of a given triangle from a given
+mesh."
+[#^Mesh mesh triangle-index]
+(let [indices (int-array 3)]
+(.getTriangle mesh triangle-index indices)
+(vec indices)))
+(defn vertex-UV-coord
+"Get the uv-coordinates of the vertex named by vertex-index"
+[#^Mesh mesh vertex-index]
+(let [UV-buffer
+(.getData
+(.getBuffer
+mesh
+VertexBuffer$Type/TexCoord))]
+[(.get UV-buffer (* vertex-index 2))
+(.get UV-buffer (+ 1 (* vertex-index 2)))]))
+(defn triangle-UV-coord
+"Get the uv-cooridnates of the triangle's verticies."
+[#^Mesh mesh width height triangle-index]
+(map (fn [[u v]] (vector (* width u) (* height v)))
+(map (partial vertex-UV-coord mesh)
+(triangle-vertex-indices mesh triangle-index))))
+(defn same-side?
+"Given the points p1 and p2 and the reference point ref, is point p
+on the same side of the line that goes through p1 and p2 as ref is?"
+[p1 p2 ref p]
+(<=
+0
+(.dot
+(.cross (.subtract p2 p1) (.subtract p p1))
+(.cross (.subtract p2 p1) (.subtract ref p1)))))
+(defn triangle-seq [#^Triangle tri]
+[(.get1 tri) (.get2 tri) (.get3 tri)])
+(defn vector3f-seq [#^Vector3f v]
+[(.getX v) (.getY v) (.getZ v)])
+(defn inside-triangle?
+"Is the point inside the triangle?"
+{:author "Dylan Holmes"}
+[#^Triangle tri #^Vector3f p]
+(let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
+(and
+(same-side? vert-1 vert-2 vert-3 p)
+(same-side? vert-2 vert-3 vert-1 p)
+(same-side? vert-3 vert-1 vert-2 p))))
+(defn triangle->matrix4f
+"Converts the triangle into a 4x4 matrix: The first three columns
+contain the vertices of the triangle; the last contains the unit
+normal of the triangle. The bottom row is filled with 1s."
+[#^Triangle t]
+(let [mat (Matrix4f.)
+[vert-1 vert-2 vert-3]
+((comp vec map) #(.get t %) (range 3))
+unit-normal (do (.calculateNormal t)(.getNormal t))
+vertices [vert-1 vert-2 vert-3 unit-normal]]
+(dorun
+(for [row (range 4) col (range 3)]
+(do
+(.set mat col row (.get (vertices row)col))
+(.set mat 3 row 1))))
+mat))
+(defn triangle-transformation
+"Returns the affine transformation that converts each vertex in the
+first triangle into the corresponding vertex in the second
+triangle."
+[#^Triangle tri-1 #^Triangle tri-2]
+(.mult
+(triangle->matrix4f tri-2)
+(.invert (triangle->matrix4f tri-1))))
+(defn point->vector2f [[u v]]
+(Vector2f. u v))
+(defn vector2f->vector3f [v]
+(Vector3f. (.getX v) (.getY v) 0))
+(defn map-triangle [f #^Triangle tri]
+(Triangle.
+(f 0 (.get1 tri))
+(f 1 (.get2 tri))
+(f 2 (.get3 tri))))
+(defn points->triangle
+"Convert a list of points into a triangle."
+[points]
+(apply #(Triangle. %1 %2 %3)
+(map (fn [point]
+(let [point (vec point)]
+(Vector3f. (get point 0 0)
+(get point 1 0)
+(get point 2 0))))
+(take 3 points))))
+(defn convex-bounds
+;;dylan
+"Returns the smallest square containing the given
+vertices, as a vector of integers [left top width height]."
+;; "Dimensions of the smallest integer bounding square of the list of
+;;  2D verticies in the form: [x y width height]."
+[uv-verts]
+(let [xs (map first uv-verts)
+ys (map second uv-verts)
+x0 (Math/floor (apply min xs))
+y0 (Math/floor (apply min ys))
+x1 (Math/ceil (apply max xs))
+y1 (Math/ceil (apply max ys))]
+[x0 y0 (- x1 x0) (- y1 y0)]))
+(defn sensors-in-triangle
+;;dylan
+"Locate the touch sensors in the triangle, returning a map of their UV and geometry-relative coordinates."
+;;"Find the locations of the touch sensors within a triangle in both
+;; UV and gemoetry relative coordinates."
+[image mesh tri-index]
+(let [width (.getWidth image)
+height (.getHeight image)
+UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
+bounds (convex-bounds UV-vertex-coords)
+cutout-triangle (points->triangle UV-vertex-coords)
+UV-sensor-coords
+(filter (comp (partial inside-triangle? cutout-triangle)
+(fn [[u v]] (Vector3f. u v 0)))
+(white-coordinates image bounds))
+UV->geometry (triangle-transformation
+cutout-triangle
+(triangle mesh tri-index))
+geometry-sensor-coords
+(map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
+UV-sensor-coords)]
+{:UV UV-sensor-coords :geometry geometry-sensor-coords}))
+(defn-memo locate-feelers
+"Search the geometry's tactile UV image for touch sensors, returning
+their positions in geometry-relative coordinates."
+[#^Geometry geo]
+(let [mesh (.getMesh geo)
+num-triangles (.getTriangleCount mesh)]
+(if-let [image (tactile-sensor-image geo)]
+(map
+(partial sensors-in-triangle image mesh)
+(range num-triangles))
+(repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
+(defn-memo touch-topology [#^Gemoetry geo]
+(vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
+(defn-memo feeler-coordinates [#^Geometry geo]
+(vec (map :geometry (locate-feelers geo))))
+(defn enable-touch [#^Geometry geo]
+(let [feeler-coords (feeler-coordinates geo)
+tris (triangles geo)
+limit 0.1
+;;results (CollisionResults.)
+]
+(if (empty? (touch-topology geo))
+nil
+(fn [node]
+(let [sensor-origins
+(map
+#(map (partial local-to-world geo) %)
+feeler-coords)
+triangle-normals
+(map (partial get-ray-direction geo)
+tris)
+rays
+(flatten
+(map (fn [origins norm]
+(map #(doto (Ray. % norm)
+(.setLimit limit)) origins))
+sensor-origins triangle-normals))]
+(vector
+(touch-topology geo)
+(vec
+(for [ray rays]
+(do
+(let [results (CollisionResults.)]
+(.collideWith node ray results)
+(let [touch-objects
+(filter #(not (= geo (.getGeometry %)))
+results)]
+(- 255
+(if (empty? touch-objects) 255
+(rem
+(int
+(* 255 (/ (.getDistance
+(first touch-objects)) limit)))
+256))))))))))))))
+(defn touch [#^Node pieces]
+(filter (comp not nil?)
+(map enable-touch
+(filter #(isa? (class %) Geometry)
+(node-seq pieces)))))
+#+end_src
+* Example
+#+name: touch-test
+#+begin_src clojure
+(ns cortex.test.touch
+(:use (cortex world util touch))
+(:import
+com.jme3.scene.shape.Sphere
+com.jme3.math.ColorRGBA
+com.jme3.math.Vector3f
+com.jme3.material.RenderState$BlendMode
+com.jme3.renderer.queue.RenderQueue$Bucket
+com.jme3.scene.shape.Box
+com.jme3.scene.Node))
+(defn ray-origin-debug
+[ray color]
+(make-shape
+(assoc base-shape
+:shape (Sphere. 5 5 0.05)
+:name "arrow"
+:color color
+:texture false
+:physical? false
+:position
+(.getOrigin ray))))
+(defn ray-debug [ray color]
+(make-shape
+(assoc
+base-shape
+:name "debug-ray"
+:physical? false
+:shape (com.jme3.scene.shape.Line.
+(.getOrigin ray)
+(.add
+(.getOrigin ray)
+(.mult (.getDirection ray)
+(float (.getLimit ray))))))))
+(defn contact-color [contacts]
+(case contacts
+0 ColorRGBA/Gray
+1  ColorRGBA/Red
+2  ColorRGBA/Green
+3  ColorRGBA/Yellow
+4  ColorRGBA/Orange
+5  ColorRGBA/Red
+6  ColorRGBA/Magenta
+7  ColorRGBA/Pink
+8  ColorRGBA/White))
+(defn update-ray-debug [node ray contacts]
+(let [origin (.getChild node 0)]
+(.setLocalTranslation origin (.getOrigin ray))
+(.setColor (.getMaterial origin) "Color" (contact-color contacts))))
+(defn init-node
+[debug-node rays]
+(.detachAllChildren debug-node)
+(dorun
+(for [ray rays]
+(do
+(.attachChild
+debug-node
+(doto (Node.)
+(.attachChild (ray-origin-debug ray ColorRGBA/Gray))
+(.attachChild (ray-debug ray ColorRGBA/Gray))
+))))))
+(defn manage-ray-debug-node [debug-node geom touch-data limit]
+(let [rays (normal-rays limit geom)]
+(if (not= (count (.getChildren debug-node)) (count touch-data))
+(init-node debug-node rays))
+(dorun
+(for [n (range (count touch-data))]
+(update-ray-debug
+(.getChild debug-node n) (nth rays n) (nth touch-data n))))))
+(defn transparent-sphere []
+(doto
+(make-shape
+(merge base-shape
+	      {:position (Vector3f. 0 2 0)
+	       :name "the blob."
+	       :material "Common/MatDefs/Misc/Unshaded.j3md"
+	       :texture "Textures/purpleWisp.png"
+	       :physical? true
+	       :mass 70
+	       :color ColorRGBA/Blue
+	       :shape (Sphere. 10 10 1)}))
+(-> (.getMaterial)
+	(.getAdditionalRenderState)
+	(.setBlendMode RenderState$BlendMode/Alpha))
+(.setQueueBucket RenderQueue$Bucket/Transparent)))
+(defn transparent-box []
+(doto
+(make-shape
+(merge base-shape
+	      {:position (Vector3f. 0 2 0)
+	       :name "box"
+	       :material "Common/MatDefs/Misc/Unshaded.j3md"
+	       :texture "Textures/purpleWisp.png"
+	       :physical? true
+	       :mass 70
+	       :color ColorRGBA/Blue
+	       :shape (Box. 1 1 1)}))
+(-> (.getMaterial)
+	(.getAdditionalRenderState)
+	(.setBlendMode RenderState$BlendMode/Alpha))
+(.setQueueBucket RenderQueue$Bucket/Transparent)))
+(defn transparent-floor []
+(doto
+(box 5 0.2 5  :mass 0 :position (Vector3f. 0 -2 0)
+:material "Common/MatDefs/Misc/Unshaded.j3md"
+:texture "Textures/redWisp.png"
+:name "floor")
+(-> (.getMaterial)
+(.getAdditionalRenderState)
+(.setBlendMode RenderState$BlendMode/Alpha))
+(.setQueueBucket RenderQueue$Bucket/Transparent)))
+(defn test-skin
+"Testing touch:
+you should see a ball which responds to the table
+and whatever balls hit it."
+[]
+(let [b
+;;(transparent-box)
+(transparent-sphere)
+;;(sphere)
+f (transparent-floor)
+debug-node (Node.)
+node      (doto (Node.) (.attachChild b) (.attachChild f))
+root-node (doto (Node.) (.attachChild node)
+(.attachChild debug-node))
+]
+(world
+root-node
+{"key-return" (fire-cannon-ball node)}
+(fn [world]
+;;  (Capture/SimpleCaptureVideo
+;;   world
+;;   (file-str "/home/r/proj/cortex/tmp/blob.avi"))
+;;  (no-logging)
+;;(enable-debug world)
+;;  (set-accuracy world (/ 1 60))
+)
+(fn [& _]
+(let [sensitivity 0.2
+touch-data (touch-percieve sensitivity b node)]
+(manage-ray-debug-node debug-node b touch-data sensitivity))
+))))
+#+end_src
+* COMMENT code generation
+#+begin_src clojure :tangle ../src/cortex/touch.clj
+<<skin-main>>
+#+end_src
+#+begin_src clojure :tangle ../src/cortex/test/touch.clj
+<<touch-test>>
+#+end_src

Mercurial > cortex

comparison org/touch.org @ 177:5af4ebe72b97