1.1 --- a/org/touch.org	Sun Feb 12 16:00:31 2012 -0700
     1.2 +++ b/org/touch.org	Mon Feb 13 06:06:02 2012 -0700
     1.3 @@ -383,10 +383,13 @@
     1.4    ;; Doing everything locally recduces garbage collection by enough to
     1.5    ;; be worth it.
     1.6    (.mult transform origin (.getOrigin ray))
     1.7 -  
     1.8    (.mult transform tip (.getDirection ray))
     1.9 -  (.subtractLocal (.getDirection ray) (.getOrigin ray)))
    1.10 +  (.subtractLocal (.getDirection ray) (.getOrigin ray))
    1.11 +  (.normalizeLocal (.getDirection ray)))
    1.12  
    1.13 +(import com.jme3.math.FastMath)
    1.14 +
    1.15 + 
    1.16  (defn touch-kernel
    1.17    "Constructs a function which will return tactile sensory data from
    1.18     'geo when called from inside a running simulation"
    1.19 @@ -397,7 +400,15 @@
    1.20            ray-reference-tips (feeler-tips geo profile)
    1.21            ray-length (tactile-scale geo)
    1.22            current-rays (map (fn [_] (Ray.)) ray-reference-origins)
    1.23 -          topology (touch-topology geo profile)]
    1.24 +          topology (touch-topology geo profile)
    1.25 +          correction (float (* ray-length -0.2))]
    1.26 +
    1.27 +      ;; slight tolerance for very close collisions.
    1.28 +      (dorun
    1.29 +       (map (fn [origin tip]
    1.30 +              (.addLocal origin (.mult (.subtract tip origin)
    1.31 +                                       correction)))
    1.32 +            ray-reference-origins ray-reference-tips))
    1.33        (dorun (map #(.setLimit % ray-length) current-rays))
    1.34        (fn [node]
    1.35          (let [transform (.getWorldMatrix geo)]
    1.36 @@ -415,11 +426,20 @@
    1.37                    (.collideWith node ray results)
    1.38                    (let [touch-objects
    1.39                          (filter #(not (= geo (.getGeometry %)))
    1.40 -                                results)]
    1.41 +                                results)
    1.42 +                        limit (.getLimit ray)]
    1.43                      [(if (empty? touch-objects)
    1.44 -                       (.getLimit ray)
    1.45 -                       (.getDistance (first touch-objects)))
    1.46 -                     (.getLimit ray)])))))))))))
    1.47 +                       limit
    1.48 +                       (let [response
    1.49 +                             (apply min (map #(.getDistance %)
    1.50 +                                             touch-objects))]
    1.51 +                         (FastMath/clamp
    1.52 +                          (float 
    1.53 +                           (if (> response limit) 0.0
    1.54 +                               (+ response correction)))
    1.55 +                           (float 0.0)
    1.56 +                           limit)))
    1.57 +                     limit])))))))))))
    1.58  
    1.59  (defn touch! 
    1.60    "Endow the creature with the sense of touch. Returns a sequence of
    1.61 @@ -433,8 +453,18 @@
    1.62                  (node-seq creature)))))
    1.63  #+end_src
    1.64  
    1.65 +#+results: kernel
    1.66 +: #'cortex.touch/touch!
    1.67 +
    1.68  * Visualizing Touch
    1.69  
    1.70 +Each feeler is represented in the image as a single pixel. The
    1.71 +grayscale value of each pixel represents how deep the feeler
    1.72 +represented by that pixel is inside another object.  Black means that
    1.73 +nothing is touching the feeler, while white means that the feeler is
    1.74 +completely inside another object, which is presumably flush with the
    1.75 +surface of the triangle from which the feeler originates.
    1.76 +
    1.77  #+name: visualization
    1.78  #+begin_src clojure
    1.79  (in-ns 'cortex.touch)
    1.80 @@ -453,9 +483,80 @@
    1.81       (let [image (points->image coords)]
    1.82         (dorun
    1.83          (for [i (range (count coords))]
    1.84 -          (.setRGB image ((coords i) 0) ((coords i) 1)
    1.85 -                   (apply touch->gray (sensor-data i))))) image))))
    1.86 +          (do
    1.87 +            (if (= i 500) (println-repl (sensor-data i)))
    1.88 +
    1.89 +            (comment
    1.90 +              (.setRGB image ((coords i) 0) ((coords i) 1)
    1.91 +                     (apply touch->gray (sensor-data i))))))) 
    1.92 +       image))))
    1.93  #+end_src
    1.94 +
    1.95 +#+results: visualization
    1.96 +: #'cortex.touch/view-touch
    1.97 +
    1.98 +* COMMENT Basic Test of Touch
    1.99 +
   1.100 +The worm's sense of touch is a bit complicated, so for this basic test
   1.101 +I'll use a new creature --- a simple cube which has touch sensors
   1.102 +evenly distributed along each of its sides.
   1.103 +
   1.104 +#+begin_src clojure
   1.105 +(in-ns 'cortex.test.touch)
   1.106 +
   1.107 +(defn touch-cube []
   1.108 +  (load-blender-model "Models/test-touch/touch-cube.blend"))
   1.109 +#+end_src
   1.110 +
   1.111 +#+begin_html
   1.112 +<br>
   1.113 +#+end_html
   1.114 +
   1.115 +#+begin_html
   1.116 +<div class="figure">
   1.117 +<center>
   1.118 +<video controls="controls" width="500">
   1.119 +  <source src="../video/touch-cube.ogg" type="video/ogg"
   1.120 +	  preload="none" poster="../images/aurellem-1280x480.png" />
   1.121 +</video>
   1.122 +</center>
   1.123 +<p>A simple creature with evenly distributed touch sensors.</p>
   1.124 +</div>
   1.125 +#+end_html
   1.126 +
   1.127 +The tactile-sensor-profile image for this simple creature looks like
   1.128 +this:
   1.129 +
   1.130 +#+attr_html: width=500
   1.131 +#+caption: The distribution of feelers along the touch-cube. The colors of the faces are irrelevant; only the white pixels specify feelers.
   1.132 +[[../images/touch-profile.png]]
   1.133 +
   1.134 +#+begin_src clojure
   1.135 +(in-ns 'cortex.test.touch)
   1.136 +
   1.137 +(defn test-basic-touch
   1.138 +  ([] (test-basic-touch false))
   1.139 +  ([record?]
   1.140 +     (let [the-cube (doto (touch-cube) (body!))
   1.141 +           touch (touch! the-cube)
   1.142 +           touch-display (view-touch)]
   1.143 +       (world (nodify [the-cube
   1.144 +                       (box 10 1 10 :position (Vector3f. 0 -10 0)
   1.145 +                            :color ColorRGBA/Gray :mass 0)])
   1.146 +
   1.147 +              standard-debug-controls
   1.148 +              
   1.149 +              (fn [world]
   1.150 +                (speed-up world)
   1.151 +                (light-up-everything world))
   1.152 +
   1.153 +              (fn [world tpf]
   1.154 +                (touch-display 
   1.155 +                 (map #(% (.getRootNode world)) touch)))))))
   1.156 +  
   1.157 +
   1.158 +#+end_src
   1.159 +
   1.160  * Adding Touch to the Worm
   1.161  
   1.162  #+name: test-touch