# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1326209884 25200
# Node ID 69174ed0f9f65535f6de0862c5cc18ed0c4791cb
# Parent  7b739503836a5978e5dae3f8047bf4f22e12ce00
Working sensor coordinate code. Dylan helped!

diff -r 7b739503836a -r 69174ed0f9f6 assets/Models/creature1/tip.png
Binary file assets/Models/creature1/tip.png has changed
diff -r 7b739503836a -r 69174ed0f9f6 assets/Models/creature1/try-again.blend
Binary file assets/Models/creature1/try-again.blend has changed
diff -r 7b739503836a -r 69174ed0f9f6 org/test-creature.org
--- a/org/test-creature.org	Mon Jan 09 19:14:47 2012 -0700
+++ b/org/test-creature.org	Tue Jan 10 08:38:04 2012 -0700
@@ -384,8 +384,9 @@
   (frame [image+]
     (frame (.getBufferedImage image+))))
 
+
 (def white -1)
-
+  
 (defn filter-pixels
   "List the coordinates of all pixels matching pred."
   {:author "Dylan Holmes"}
@@ -414,38 +415,162 @@
     (.cross (.subtract p2 p1) (.subtract p p1))
     (.cross (.subtract p2 p1) (.subtract ref p1)))))
 
+
+(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."
+  [#^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))))
+
+(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 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 vector3f-seq [#^Vector3f v]
+  [(.getX v) (.getY v) (.getZ v)])
+
 (defn inside-triangle?
-  [vert-1 vert-2 vert-3 p]
-   (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)))
+  "Is the point inside the triangle? Now what do we do?
+   You might want to hold on there"
+  {:author "God"}
+  [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 analyze-triangle [#^Geometry obj #^Triangle tri]
-  ;; first, calculate the transformation matrix that will take us
-  ;; from uv-coordinates to the real world coordinates
-  (let [mesh (.getMesh obj)
-        world [(.get1 tri) (.get2 tri) (.get3 tri)]
-        uv    (tri-uv-coord mesh tri)
+(defn pixel-triangle
+  "Convert the mesh triange 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)))
 
-    
+(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))]
 
-    
-   (println-repl world uv)))
-  
-  
+    [x y 
+     (- (apply max (map first verts)) x)
+     (- (apply max (map second verts)) y)
+     ]))
   
 
+(defn locate-tactile-sensors
+  "Search the geometry's tactile UV image for touch sensors, returning
+  their positions in geometry-relative coordinates."
+  [#^Geometry geo]
 
-(defn tactile-coords* 
-  [#^Geometry obj]
-  (let
-      [tris (triangles obj)
-       mesh (.getMesh obj)
-       
-    
-  )
+  ;; inside-triangle? white-coordinates triangle-transformation 
+  ;; tri-uv-coord touch-receptor-image
+  (let [mesh (.getMesh geo)
+        image (touch-receptor-image geo)
+        width (.getWidth image)
+        height (.getHeight image)
+        tris (triangles geo)
+
+        ;; 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))))]
+    (map sensor-coords tris)))
+
+
+            
+          
+          
+        
+        
+ 
+
+
 
 
 
@@ -485,7 +610,6 @@
                                 (* height (.getY uv-3)))
              left-corner
              (Vector2f. min-x min-y)
-             
              ]
 
          (.setRoi receptors min-x min-y (- max-x min-x) (- max-y min-y))
@@ -791,3 +915,91 @@
 <<body-1>>
 #+end_src
 
+
+
+
+
+(defn transform-trianglesdsd
+  "Transform that converts each vertex in the first triangle
+  into the corresponding vertex in the second triangle."
+  [#^Triangle tri-1 #^Triangle tri-2]
+  (let [in  [(.get1 tri-1)
+             (.get2 tri-1)
+             (.get3 tri-1)]
+        out [(.get1 tri-2)
+             (.get2 tri-2)
+             (.get3 tri-2)]]
+    (let [translate (doto (Matrix4f.) (.setTranslation (.negate (in 0))))
+          in* [(.mult translate (in 0))
+               (.mult translate (in 1))
+               (.mult translate (in 2))]
+          final-translation
+          (doto (Matrix4f.)
+            (.setTranslation (out 1)))
+          
+          rotate-1
+          (doto (Matrix3f.)
+            (.fromStartEndVectors
+             (.normalize
+              (.subtract
+               (in* 1) (in* 0)))
+             (.normalize
+              (.subtract
+               (out 1) (out 0)))))
+          in** [(.mult rotate-1 (in* 0))
+                (.mult rotate-1 (in* 1))
+                (.mult rotate-1 (in* 2))]
+          scale-factor-1
+          (.mult
+           (.normalize
+            (.subtract
+             (out 1)
+             (out 0)))
+           (/ (.length
+               (.subtract (out 1)
+                          (out 0)))
+              (.length
+               (.subtract (in** 1)
+                          (in** 0)))))
+          scale-1 (doto (Matrix4f.) (.setScale scale-factor-1))
+          in*** [(.mult scale-1 (in** 0))
+                 (.mult scale-1 (in** 1))
+                 (.mult scale-1 (in** 2))]
+          
+          
+          
+          
+          
+          ]
+      
+      (dorun (map println in))
+      (println)
+      (dorun (map println in*))
+      (println)
+      (dorun (map println in**))
+      (println)
+      (dorun (map println in***))
+      (println)
+      
+    )))
+        
+                   
+                     
+                                
+                        
+                                   
+        
+                 
+        
+
+
+        
+                     
+                      
+                   
+                   
+    
+  
+  )
+  
+