Mercurial > cortex

comparison org/touch.org @ 242:a7f26a074071

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
saving progress...
author Robert McIntyre <rlm@mit.edu>
date Sun, 12 Feb 2012 13:48:28 -0700
parents f2e583be8584
children f33fec68f775
comparison
equal deleted inserted replaced
241:f2e583be8584 242:a7f26a074071
132 #+begin_src clojure 132 #+begin_src clojure
133 (in-ns 'cortex.touch) 133 (in-ns 'cortex.touch)
134 134
135 (declare touch-topology feelers set-ray) 135 (declare touch-topology feelers set-ray)
136 136
137 (defn set-ray [#^Ray ray #^Matrix4f transform
138 #^Vector3f origin #^Vector3f tip
139 length]
140 (.setOrigin ray (.mult transform origin))
141 (.setDirection ray (.subtract
142 (.mult transform tip)
143 (.getOrigin ray)))
144 (.setLimit ray length)
145 ray)
146
147
137 (defn touch-kernel 148 (defn touch-kernel
138 "Constructs a function which will return tactile sensory data from 149 "Constructs a function which will return tactile sensory data from
139 'geo when called from inside a running simulation" 150 'geo when called from inside a running simulation"
140 [#^Geometry geo] 151 [#^Geometry geo]
141 (let [[ray-reference-origins 152 (let [profile (tactile-sensor-profile geo)
142 ray-reference-tips 153 ray-reference-origins (feeler-origins geo profile)
143 ray-lengths] (feelers geo) 154 ray-reference-tips (feeler-tips geo profile)
155 ray-lengths (repeat 9000 0.1)
144 current-rays (map (fn [] (Ray.)) ray-reference-origins) 156 current-rays (map (fn [] (Ray.)) ray-reference-origins)
145 topology (touch-topology geo)] 157 topology (touch-topology geo profile)]
146 (if (empty? ray-reference-origins) nil 158 (if (empty? ray-reference-origins) nil
147 (fn [node] 159 (fn [node]
148 (let [transform (.getWorldMatrix geo)] 160 (let [transform (.getWorldMatrix geo)]
149 (dorun 161 (dorun
150 (map (fn [ray ref-origin ref-tip length] 162 (map (fn [ray ref-origin ref-tip length]
237 (inside-triangle? (->triangle pixel-triangle) 249 (inside-triangle? (->triangle pixel-triangle)
238 (->vector3f coord))) 250 (->vector3f coord)))
239 (white-coordinates image (convex-bounds pixel-triangle)))) 251 (white-coordinates image (convex-bounds pixel-triangle))))
240 (pixel-triangles geo image))) 252 (pixel-triangles geo image)))
241 253
242 (defn feeler-origins [#^Geometry geo image] 254 (defn feeler-world-coords [#^Geometry geo image]
243 (let [transforms 255 (let [transforms
244 (map #(triangles->affine-transform 256 (map #(triangles->affine-transform
245 (->triangle %1) (->triangle %2)) 257 (->triangle %1) (->triangle %2))
246 (pixel-triangles geo image) 258 (pixel-triangles geo image)
247 (triangles geo))] 259 (triangles geo))]
248 (mapcat (fn [transform coords] 260 (map (fn [transform coords]
249 (map #(.mult transform (->vector3f %)) coords)) 261 (map #(.mult transform (->vector3f %)) coords))
250 transforms (feeler-pixel-coords geo image)))) 262 transforms (feeler-pixel-coords geo image))))
251 263
264 (defn feeler-origins [#^Geometry geo image]
265 (reduce concat (feeler-world-coords geo image)))
266
252 (defn feeler-tips [#^Geometry geo image] 267 (defn feeler-tips [#^Geometry geo image]
253 (let [origins (feeler-origins geo image) 268 (let [world-coords (feeler-world-coords geo image)
254 normals 269 normals
255 (map 270 (map
256 (fn [triangle] 271 (fn [triangle]
257 (.calculateNormal triangle) 272 (.calculateNormal triangle)
258 (.clone (.getNormal triangle))) 273 (.clone (.getNormal triangle)))
259 (map ->triangle (triangles geo)))] 274 (map ->triangle (triangles geo)))]
260 (map #(.add %1 %2) origins normals))) 275
276 (mapcat (fn [origins normal]
277 (map #(.add % normal) origins))
278 world-coords normals)))
279
261 280
262 (defn touch-topology [#^Geometry geo image] 281 (defn touch-topology [#^Geometry geo image]
263 (collapse (feeler-pixel-coords geo image))) 282 (collapse (feeler-pixel-coords geo image)))
264 283
265 284