#+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

 

 Touch is critical to navigation and spatial reasoning and as such I

 need a simulated version of it to give to my AI creatures.

 

 #+name: skin-main

 #+begin_src clojure

 (in-ns 'cortex.touch)

 

 (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-profile

   "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")]

     (load-image image-path)))

 

 (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

   "Returns the smallest square containing the given vertices, as a

    vector of integers [left top 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

   "Locate the touch sensors in the triangle, returning a map of their

    UV and geometry-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 profile 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-profile geo)]

       (map

        (partial sensors-in-triangle image mesh)

        (range num-triangles))

       (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))

 

 (defn-memo touch-topology

   "Return a sequence of vectors of the form [x y] describing the

    \"topology\" of the tactile sensors. Points that are close together

    in the touch-topology are generally close together in the simulation."

   [#^Gemoetry geo]

   (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))

 

 (defn-memo feeler-coordinates

   "The location of the touch sensors in world-space coordinates."

   [#^Geometry geo]

   (vec (map :geometry (locate-feelers geo))))

 

 (defn touch-fn

   "Returns a function which returns tactile sensory data when called

    inside a running simulation."

   [#^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! 

   "Endow the creature with the sense of touch. Returns a sequence of

    functions, one for each body part with a tactile-sensor-proile,

    each of which when called returns sensory data for that body part."

   [#^Node creature]

   (filter

    (comp not nil?)

    (map touch-fn

         (filter #(isa? (class %) Geometry)

                 (node-seq creature)))))

 

 (defn view-touch 

   "Creates a function which accepts a list of  touch sensor-data and

    displays each element to the screen."

   []

   (view-sense

    (fn 

      [[coords sensor-data]]

      (let [image (points->image coords)]

        (dorun

         (for [i (range (count coords))]

           (.setRGB image ((coords i) 0) ((coords i) 1)

                    (gray (sensor-data i)))))

        image))))

 #+end_src

 

 * Headers 

 #+begin_src clojure

 (ns cortex.touch

   "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry

   to be outfitted with touch sensors with density determined by a UV

   image. In this way a Geometry can know what parts of itself are

   touching nearby objects. Reads specially prepared blender files to

   construct this sense automatically."

   {:author "Robert McIntyre"}

   (:use (cortex world util sense))

   (:use clojure.contrib.def)

   (:import (com.jme3.scene Geometry Node Mesh))

   (:import com.jme3.collision.CollisionResults)

   (:import com.jme3.scene.VertexBuffer$Type)

   (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))

 #+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 

 #+end_src