# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1329079708 25200
# Node ID a7f26a07407105fce36055e48bea2e70f3aee975
# Parent  f2e583be858460969a4744db027e9f5b4edd9a1f
saving progress...

diff -r f2e583be8584 -r a7f26a074071 org/touch.org
--- a/org/touch.org	Sun Feb 12 13:30:42 2012 -0700
+++ b/org/touch.org	Sun Feb 12 13:48:28 2012 -0700
@@ -134,15 +134,27 @@
 
 (declare touch-topology feelers set-ray)
 
+(defn set-ray [#^Ray ray #^Matrix4f transform
+               #^Vector3f origin #^Vector3f tip
+               length]
+  (.setOrigin ray (.mult transform origin))
+  (.setDirection  ray (.subtract
+                       (.mult transform tip)
+                       (.getOrigin ray)))
+  (.setLimit ray length)
+  ray)
+
+
 (defn touch-kernel
   "Constructs a function which will return tactile sensory data from
    'geo when called from inside a running simulation"
   [#^Geometry geo]
-  (let [[ray-reference-origins
-         ray-reference-tips
-         ray-lengths] (feelers geo)
+  (let [profile (tactile-sensor-profile geo)
+        ray-reference-origins (feeler-origins geo profile)
+        ray-reference-tips (feeler-tips geo profile)
+        ray-lengths (repeat 9000 0.1)
         current-rays (map (fn [] (Ray.)) ray-reference-origins)
-        topology (touch-topology geo)]
+        topology (touch-topology geo profile)]
     (if (empty? ray-reference-origins) nil
         (fn [node]
           (let [transform (.getWorldMatrix geo)]
@@ -239,25 +251,32 @@
        (white-coordinates image (convex-bounds pixel-triangle))))
   (pixel-triangles geo image)))
 
-(defn feeler-origins [#^Geometry geo image]
+(defn feeler-world-coords [#^Geometry geo image]
   (let [transforms
         (map #(triangles->affine-transform
                (->triangle %1) (->triangle %2))
              (pixel-triangles geo image)
              (triangles geo))]
-    (mapcat (fn [transform coords]
+    (map (fn [transform coords]
               (map #(.mult transform (->vector3f %)) coords))
             transforms (feeler-pixel-coords geo image))))
 
+(defn feeler-origins [#^Geometry geo image]
+   (reduce concat (feeler-world-coords geo image)))
+
 (defn feeler-tips [#^Geometry geo image]
-  (let [origins (feeler-origins geo image)
+  (let [world-coords (feeler-world-coords geo image)
         normals
         (map
          (fn [triangle]
            (.calculateNormal triangle)
            (.clone (.getNormal triangle)))
          (map ->triangle (triangles geo)))]
-    (map #(.add %1 %2) origins normals)))
+
+    (mapcat (fn [origins normal]
+              (map #(.add % normal) origins))
+            world-coords normals)))
+    
 
 (defn touch-topology [#^Geometry geo image]
   (collapse (feeler-pixel-coords geo image)))