# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1326625711 25200
# Node ID 92b857b6145d78112689129337409220b1ac0c28
# Parent  53fb379ac67824b306bffbaa54a8782df1172726
slow implementation of UV-mapped touch is complete

diff -r 53fb379ac678 -r 92b857b6145d assets/Models/creature1/tip.png
Binary file assets/Models/creature1/tip.png has changed
diff -r 53fb379ac678 -r 92b857b6145d assets/Models/creature1/try-again.blend
Binary file assets/Models/creature1/try-again.blend has changed
diff -r 53fb379ac678 -r 92b857b6145d org/test-creature.org
--- a/org/test-creature.org	Sun Jan 15 00:40:49 2012 -0700
+++ b/org/test-creature.org	Sun Jan 15 04:08:31 2012 -0700
@@ -65,6 +65,7 @@
 (import java.awt.Dimension)
 (import com.aurellem.capture.RatchetTimer)
 (declare joint-create)
+(use 'clojure.contrib.def)
 
 (defn view-image
   "Initailizes a JPanel on which you may draw a BufferedImage.
@@ -142,40 +143,41 @@
   "Take a set of pairs of integers and collapse them into a
    contigous bitmap."
   [points]
-  (let
-      [num-points (count points)
-       center (vector
-               (int (average (map first points)))
-               (int (average (map first points))))
-       flattened
-       (reduce
-        concat 
-        (map
-         (fn [column]
-           (map vector
-                (map first column)
-                (collapse-1d (second center)
-                             (map second column))))
-         (partition-by first (sort-by first points))))
-       squeezed
-       (reduce
-        concat 
-        (map
-         (fn [row]
-           (map vector
-                (collapse-1d (first center)
-                             (map first row))
-                (map second row)))
-         (partition-by second (sort-by second flattened))))
-       relocate
-       (let [min-x (apply min (map first squeezed))
-             min-y (apply min (map second squeezed))]
-         (map (fn [[x y]]
-                [(- x min-x)
-                 (- y min-y)])
-              squeezed))]
-    relocate
-   ))
+  (if (empty? points) []
+      (let
+          [num-points (count points)
+           center (vector
+                   (int (average (map first points)))
+                   (int (average (map first points))))
+           flattened
+           (reduce
+            concat 
+            (map
+             (fn [column]
+               (map vector
+                    (map first column)
+                    (collapse-1d (second center)
+                                 (map second column))))
+             (partition-by first (sort-by first points))))
+           squeezed
+           (reduce
+            concat 
+            (map
+             (fn [row]
+               (map vector
+                    (collapse-1d (first center)
+                                 (map first row))
+                    (map second row)))
+             (partition-by second (sort-by second flattened))))
+           relocate
+           (let [min-x (apply min (map first squeezed))
+                 min-y (apply min (map second squeezed))]
+             (map (fn [[x y]]
+                    [(- x min-x)
+                     (- y min-y)])
+                  squeezed))]
+        relocate
+        )))
 
 (defn load-bullet []
   (let [sim (world (Node.) {} no-op no-op)]
@@ -417,33 +419,10 @@
 
 (import ij.ImagePlus)
 
-(defn triangle-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)))
+;; Every Mesh has many triangles, each with its own index.
+;; Every vertex has its own index as well.
 
-(defn 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))]
-    (Vector2f.
-     (.get UV-buffer (* vertex-index 2))
-     (.get UV-buffer (+ 1 (* vertex-index 2))))))
-
-(defn tri-uv-coord
-  "Get the uv-cooridnates of the triangle's verticies."
-  [#^Mesh mesh #^Triangle triangle]
-  (map (partial uv-coord mesh)
-       (triangle-indices mesh (.getIndex triangle))))
-
-(defn touch-receptor-image
+(defn tactile-sensor-image
   "Return the touch-sensor distribution image in ImagePlus format, or
    nil if it does not exist."
   [#^Geometry obj]
@@ -456,9 +435,7 @@
           [texture
            (.getTextureValue texture-param)
            im (.getImage texture)]
-        (ImagePlus.
-         "UV-map"
-         (ImageToAwt/convert im false false 0))))))
+        (ImageToAwt/convert im false false 0)))))
 
 (import ij.process.ImageProcessor)
 (import java.awt.image.BufferedImage)
@@ -466,25 +443,58 @@
 (def white -1)
   
 (defn filter-pixels
-  "List the coordinates of all pixels matching pred."
+  "List the coordinates of all pixels matching pred, within the bounds
+   provided. Bounds -> [x0 y0 width height]"
   {:author "Dylan Holmes"}
-  [pred #^ImageProcessor ip]
-  (let
-      [width (.getWidth ip)
-       height (.getHeight ip)]
-    ((fn accumulate [x y matches]
-       (cond
-        (>= y height) matches
-        (>= x width) (recur 0 (inc y) matches)
-        (pred (.getPixel ip x y))
-        (recur (inc x) y (conj matches (Vector2f. x y)))
-        :else (recur (inc x) y matches)))
-     0 0 [])))
+  ([pred #^BufferedImage image]
+     (filter-pixels pred image [0 0 (.getWidth image) (.getHeight image)]))
+  ([pred #^BufferedImage image [x0 y0 width height]]
+     ((fn accumulate [x y matches]
+        (cond
+         (>= y (+ height y0)) matches
+         (>= x (+ width x0)) (recur 0 (inc y) matches)
+         (pred (.getRGB image x y))
+         (recur (inc x) y (conj matches [x y]))
+         :else (recur (inc x) y matches)))
+      x0 y0 [])))
 
 (defn white-coordinates
-  "List the coordinates of all the white pixels in an image."
-  [#^ImageProcessor ip]
-  (filter-pixels #(= % white) ip))
+  "Coordinates of all the white pixels in a subset of the image."
+  [#^BufferedImage image bounds]
+  (filter-pixels #(= % white) image bounds))
+
+(defn triangle
+  "Get the triangle specified by triangle-index from the mesh"
+  [#^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
@@ -496,11 +506,26 @@
     (.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 of vertices: 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."
+  "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]
@@ -523,150 +548,85 @@
    (triangle->matrix4f tri-2)
    (.invert (triangle->matrix4f tri-1))))
 
-(def death (Triangle. 
-            (Vector3f. 1 1 1)
-            (Vector3f. 1 2 3)
-            (Vector3f. 5 6 7)))
-
-(def death-2 (Triangle. 
-             (Vector3f. 2 2 2)
-             (Vector3f. 1 1 1)
-             (Vector3f. 0 1 0)))
+(defn point->vector2f [[u v]]
+  (Vector2f. u v))
 
 (defn vector2f->vector3f [v]
   (Vector3f. (.getX v) (.getY v) 0))
 
-(extend-type Triangle
-  Textual
-  (text [t]
-    (println "Triangle: " \newline (.get1 t) \newline
-             (.get2 t) \newline (.get3 t))))
-
 (defn map-triangle [f #^Triangle tri]
   (Triangle.
    (f 0 (.get1 tri))
    (f 1 (.get2 tri))
    (f 2 (.get3 tri))))
 
-(defn triangle-seq [#^Triangle tri]
-  [(.get1 tri) (.get2 tri) (.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 vector3f-seq [#^Vector3f v]
-  [(.getX v) (.getY v) (.getZ v)])
-
-(defn inside-triangle?
-  "Is the point inside the triangle? Now what do we do?
-   You might want to hold on there"
-  {:author "Dylan Holmes"}
-  [tri 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 uv-triangle
-  "Convert the mesh triangle into the cooresponding triangle in
-   UV-space. Z-component of these triangles is always zero." 
-  [#^Mesh mesh #^Triangle tri]
-  (apply #(Triangle. %1 %2 %3)
-         (map vector2f->vector3f
-              (tri-uv-coord mesh tri))))
-
-(defn pixel-triangle
-  "Convert the mesh triangle into the corresponding triangle in
-  UV-pixel-space. Z compenent will be zero."
-  [#^Mesh mesh #^Triangle tri width height]
-  (map-triangle (fn [_ v]
-                  (Vector3f. (* width (.getX v))
-                             (* height (.getY v))
-                             0))
-                (uv-triangle mesh tri)))
-
-(def rasterize pixel-triangle)
-
-
-(defn triangle-bounds
-  "Dimensions of the bounding square of the triangle in the form
-   [x y width height].
-   Assumes that the triangle lies in the XY plane."
-  [#^Triangle tri]
-  (let [verts (map vector3f-seq (triangle-seq tri))
-        x (apply min (map first verts))
-        y (apply min (map second verts))]
-    [x y 
-     (- (apply max (map first verts)) x)
-     (- (apply max (map second verts)) y)
-     ]))
-  
+(defn convex-bounds
+  "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
   "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)]
+        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 locate-feelers
+(defn-memo locate-feelers
   "Search the geometry's tactile UV image for touch sensors, returning
   their positions in geometry-relative coordinates."
   [#^Geometry geo]
-  (if-let [image (touch-receptor-image geo)]
-    (let [mesh (.getMesh geo)
-          tris (triangles geo)
-
-          width (.getWidth image)
-          height (.getHeight image)
-          
-          ;; for each triangle
-          sensor-coords
-          (fn [tri]
-            ;; translate triangle to uv-pixel-space
-            (let [uv-tri
-                  (pixel-triangle mesh tri width height)
-                  bounds (vec (triangle-bounds uv-tri))]
-              
-              ;; get that part of the picture
-              
-              (apply #(.setRoi image %1 %2 %3 %4) bounds)
-              (let [cutout (.crop (.getProcessor image))
-                    ;; extract white pixels inside triangle
-                    cutout-tri
-                    (map-triangle
-                     (fn [_ v]
-                       (.subtract
-                        v
-                        (Vector3f. (bounds 0) (bounds 1) (float 0))))
-                     uv-tri)
-                    whites (filter (partial inside-triangle? cutout-tri)
-                                   (map vector2f->vector3f
-                                        (white-coordinates cutout)))
-                    ;; translate pixel coordinates to world-space
-                    transform (triangle-transformation cutout-tri tri)]
-                (map #(.mult transform %) whites))))]
-      (vec (map sensor-coords tris)))
-    (repeat (count (triangles 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}))))
 
 (use 'clojure.contrib.def)
 
 (defn-memo touch-topology [#^Gemoetry geo]
-  (if-let [image (touch-receptor-image geo)]
-    (let [feeler-coords
-          (map
-           #(vector (int (.getX %)) (int (.getY %)))
-           (white-coordinates
-            (.getProcessor image)))]
-      (vec (collapse feeler-coords)))
-    []))
+  (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 (locate-feelers geo)
+  (let [feeler-coords (feeler-coordinates geo)
         tris (triangles geo)
         limit 0.1]
     (fn [node]
@@ -744,6 +704,7 @@
      ;;(com.aurellem.capture.Capture/captureVideo
      ;; world (file-str "/home/r/proj/ai-videos/hand"))
      (.setTimer world (RatchetTimer. 60))
+     ;;(speed-up world)
      ;;(set-gravity world (Vector3f. 0 0 0))
      )
    (fn [world tpf]
diff -r 53fb379ac678 -r 92b857b6145d org/util.org
--- a/org/util.org	Sun Jan 15 00:40:49 2012 -0700
+++ b/org/util.org	Sun Jan 15 04:08:31 2012 -0700
@@ -453,6 +453,13 @@
   Textual
   (text [control]
     (println "...geo...")))
+
+(extend-type Triangle
+  Textual
+  (text [t]
+    (println "Triangle: " \newline (.get1 t) \newline
+             (.get2 t) \newline (.get3 t))))
+
 #+end_src
 
 Here I make the =Viewable= protocol and extend it to JME's types.  Now