1.1 --- a/org/touch.org	Sun Feb 12 13:30:42 2012 -0700
     1.2 +++ b/org/touch.org	Sun Feb 12 13:48:28 2012 -0700
     1.3 @@ -134,15 +134,27 @@
     1.4  
     1.5  (declare touch-topology feelers set-ray)
     1.6  
     1.7 +(defn set-ray [#^Ray ray #^Matrix4f transform
     1.8 +               #^Vector3f origin #^Vector3f tip
     1.9 +               length]
    1.10 +  (.setOrigin ray (.mult transform origin))
    1.11 +  (.setDirection  ray (.subtract
    1.12 +                       (.mult transform tip)
    1.13 +                       (.getOrigin ray)))
    1.14 +  (.setLimit ray length)
    1.15 +  ray)
    1.16 +
    1.17 +
    1.18  (defn touch-kernel
    1.19    "Constructs a function which will return tactile sensory data from
    1.20     'geo when called from inside a running simulation"
    1.21    [#^Geometry geo]
    1.22 -  (let [[ray-reference-origins
    1.23 -         ray-reference-tips
    1.24 -         ray-lengths] (feelers geo)
    1.25 +  (let [profile (tactile-sensor-profile geo)
    1.26 +        ray-reference-origins (feeler-origins geo profile)
    1.27 +        ray-reference-tips (feeler-tips geo profile)
    1.28 +        ray-lengths (repeat 9000 0.1)
    1.29          current-rays (map (fn [] (Ray.)) ray-reference-origins)
    1.30 -        topology (touch-topology geo)]
    1.31 +        topology (touch-topology geo profile)]
    1.32      (if (empty? ray-reference-origins) nil
    1.33          (fn [node]
    1.34            (let [transform (.getWorldMatrix geo)]
    1.35 @@ -239,25 +251,32 @@
    1.36         (white-coordinates image (convex-bounds pixel-triangle))))
    1.37    (pixel-triangles geo image)))
    1.38  
    1.39 -(defn feeler-origins [#^Geometry geo image]
    1.40 +(defn feeler-world-coords [#^Geometry geo image]
    1.41    (let [transforms
    1.42          (map #(triangles->affine-transform
    1.43                 (->triangle %1) (->triangle %2))
    1.44               (pixel-triangles geo image)
    1.45               (triangles geo))]
    1.46 -    (mapcat (fn [transform coords]
    1.47 +    (map (fn [transform coords]
    1.48                (map #(.mult transform (->vector3f %)) coords))
    1.49              transforms (feeler-pixel-coords geo image))))
    1.50  
    1.51 +(defn feeler-origins [#^Geometry geo image]
    1.52 +   (reduce concat (feeler-world-coords geo image)))
    1.53 +
    1.54  (defn feeler-tips [#^Geometry geo image]
    1.55 -  (let [origins (feeler-origins geo image)
    1.56 +  (let [world-coords (feeler-world-coords geo image)
    1.57          normals
    1.58          (map
    1.59           (fn [triangle]
    1.60             (.calculateNormal triangle)
    1.61             (.clone (.getNormal triangle)))
    1.62           (map ->triangle (triangles geo)))]
    1.63 -    (map #(.add %1 %2) origins normals)))
    1.64 +
    1.65 +    (mapcat (fn [origins normal]
    1.66 +              (map #(.add % normal) origins))
    1.67 +            world-coords normals)))
    1.68 +    
    1.69  
    1.70  (defn touch-topology [#^Geometry geo image]
    1.71    (collapse (feeler-pixel-coords geo image)))