view org/test-creature.org @ 97:2ff8c7c4e64d

have function for creating touch for a blender creature
author Robert McIntyre <rlm@mit.edu>
date Wed, 11 Jan 2012 00:06:33 -0700
parents 4a9096f31017
children 5b23961433e3
line wrap: on
line source
1 #+title: First attempt at a creature!
2 #+author: Robert McIntyre
3 #+email: rlm@mit.edu
4 #+description:
5 #+keywords: simulation, jMonkeyEngine3, clojure
6 #+SETUPFILE: ../../aurellem/org/setup.org
7 #+INCLUDE: ../../aurellem/org/level-0.org
9 * Intro
10 So far, I've made the following senses --
11 - Vision
12 - Hearing
13 - Touch
14 - Proprioception
16 And one effector:
17 - Movement
19 However, the code so far has only enabled these senses, but has not
20 actually implemented them. For example, there is still a lot of work
21 to be done for vision. I need to be able to create an /eyeball/ in
22 simulation that can be moved around and see the world from different
23 angles. I also need to determine weather to use log-polar or cartesian
24 for the visual input, and I need to determine how/wether to
25 disceritise the visual input.
27 I also want to be able to visualize both the sensors and the
28 effectors in pretty pictures. This semi-retarted creature will by my
29 first attempt at bringing everything together.
31 * The creature's body
33 Still going to do an eve-like body in blender, but due to problems
34 importing the joints, etc into jMonkeyEngine3, I',m going to do all
35 the connecting here in clojure code, using the names of the individual
36 components and trial and error. Later, I'll maybe make some sort of
37 creature-building modifications to blender that support whatever
38 discreitized senses I'm going to make.
40 #+name: body-1
41 #+begin_src clojure
42 (ns cortex.silly
43 "let's play!"
44 {:author "Robert McIntyre"})
46 ;; TODO remove this!
47 (require 'cortex.import)
48 (cortex.import/mega-import-jme3)
49 (use '(cortex world util body hearing touch vision))
51 (rlm.rlm-commands/help)
53 (declare joint-create)
55 (defn load-bullet []
56 (let [sim (world (Node.) {} no-op no-op)]
57 (.enqueue
58 sim
59 (fn []
60 (.stop sim)))
61 (.start sim)))
63 (defn load-blender-model
64 "Load a .blend file using an asset folder relative path."
65 [^String model]
66 (.loadModel
67 (doto (asset-manager)
68 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
69 model))
71 (defn meta-data [blender-node key]
72 (if-let [data (.getUserData blender-node "properties")]
73 (.findValue data key)
74 nil))
76 (defn blender-to-jme
77 "Convert from Blender coordinates to JME coordinates"
78 [#^Vector3f in]
79 (Vector3f. (.getX in)
80 (.getZ in)
81 (- (.getY in))))
83 (defn jme-to-blender
84 "Convert from JME coordinates to Blender coordinates"
85 [#^Vector3f in]
86 (Vector3f. (.getX in)
87 (- (.getZ in))
88 (.getY in)))
90 (defn joint-targets
91 "Return the two closest two objects to the joint object, ordered
92 from bottom to top according to the joint's rotation."
93 [#^Node parts #^Node joint]
94 ;;(println (meta-data joint "joint"))
95 (.getWorldRotation joint)
96 (loop [radius (float 0.01)]
97 (let [results (CollisionResults.)]
98 (.collideWith
99 parts
100 (BoundingBox. (.getWorldTranslation joint)
101 radius radius radius)
102 results)
103 (let [targets
104 (distinct
105 (map #(.getGeometry %) results))]
106 (if (>= (count targets) 2)
107 (sort-by
108 #(let [v
109 (jme-to-blender
110 (.mult
111 (.inverse (.getWorldRotation joint))
112 (.subtract (.getWorldTranslation %)
113 (.getWorldTranslation joint))))]
114 (println-repl (.getName %) ":" v)
115 (.dot (Vector3f. 1 1 1)
116 v))
117 (take 2 targets))
118 (recur (float (* radius 2))))))))
120 (defn world-to-local
121 "Convert the world coordinates into coordinates relative to the
122 object (i.e. local coordinates), taking into account the rotation
123 of object."
124 [#^Spatial object world-coordinate]
125 (let [out (Vector3f.)]
126 (.worldToLocal object world-coordinate out) out))
128 (defn local-to-world
129 "Convert the local coordinates into coordinates into world relative
130 coordinates"
131 [#^Spatial object local-coordinate]
132 (let [world-coordinate (Vector3f.)]
133 (.localToWorld object local-coordinate world-coordinate)
134 world-coordinate))
137 (defmulti joint-dispatch
138 "Translate blender pseudo-joints into real JME joints."
139 (fn [constraints & _]
140 (:type constraints)))
142 (defmethod joint-dispatch :point
143 [constraints control-a control-b pivot-a pivot-b rotation]
144 (println-repl "creating POINT2POINT joint")
145 (Point2PointJoint.
146 control-a
147 control-b
148 pivot-a
149 pivot-b))
151 (defmethod joint-dispatch :hinge
152 [constraints control-a control-b pivot-a pivot-b rotation]
153 (println-repl "creating HINGE joint")
154 (let [axis
155 (if-let
156 [axis (:axis constraints)]
157 axis
158 Vector3f/UNIT_X)
159 [limit-1 limit-2] (:limit constraints)
160 hinge-axis
161 (.mult
162 rotation
163 (blender-to-jme axis))]
164 (doto
165 (HingeJoint.
166 control-a
167 control-b
168 pivot-a
169 pivot-b
170 hinge-axis
171 hinge-axis)
172 (.setLimit limit-1 limit-2))))
174 (defmethod joint-dispatch :cone
175 [constraints control-a control-b pivot-a pivot-b rotation]
176 (let [limit-xz (:limit-xz constraints)
177 limit-xy (:limit-xy constraints)
178 twist (:twist constraints)]
180 (println-repl "creating CONE joint")
181 (println-repl rotation)
182 (println-repl
183 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
184 (println-repl
185 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
186 (println-repl
187 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
188 (doto
189 (ConeJoint.
190 control-a
191 control-b
192 pivot-a
193 pivot-b
194 rotation
195 rotation)
196 (.setLimit (float limit-xz)
197 (float limit-xy)
198 (float twist)))))
200 (defn connect
201 "here are some examples:
202 {:type :point}
203 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
204 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
206 {:type :cone :limit-xz 0]
207 :limit-xy 0]
208 :twist 0]} (use XZY rotation mode in blender!)"
209 [#^Node obj-a #^Node obj-b #^Node joint]
210 (let [control-a (.getControl obj-a RigidBodyControl)
211 control-b (.getControl obj-b RigidBodyControl)
212 joint-center (.getWorldTranslation joint)
213 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
214 pivot-a (world-to-local obj-a joint-center)
215 pivot-b (world-to-local obj-b joint-center)]
217 (if-let [constraints
218 (map-vals
219 eval
220 (read-string
221 (meta-data joint "joint")))]
222 ;; A side-effect of creating a joint registers
223 ;; it with both physics objects which in turn
224 ;; will register the joint with the physics system
225 ;; when the simulation is started.
226 (do
227 (println-repl "creating joint between"
228 (.getName obj-a) "and" (.getName obj-b))
229 (joint-dispatch constraints
230 control-a control-b
231 pivot-a pivot-b
232 joint-rotation))
233 (println-repl "could not find joint meta-data!"))))
235 (defn assemble-creature [#^Node pieces joints]
236 (dorun
237 (map
238 (fn [geom]
239 (let [physics-control
240 (RigidBodyControl.
241 (HullCollisionShape.
242 (.getMesh geom))
243 (if-let [mass (meta-data geom "mass")]
244 (do
245 (println-repl
246 "setting" (.getName geom) "mass to" (float mass))
247 (float mass))
248 (float 1)))]
250 (.addControl geom physics-control)))
251 (filter #(isa? (class %) Geometry )
252 (node-seq pieces))))
254 (dorun
255 (map
256 (fn [joint]
257 (let [[obj-a obj-b]
258 (joint-targets pieces joint)]
259 (connect obj-a obj-b joint)))
260 joints))
261 pieces)
263 (defn blender-creature [blender-path]
264 (let [model (load-blender-model blender-path)
265 joints
266 (if-let [joint-node (.getChild model "joints")]
267 (seq (.getChildren joint-node))
268 (do (println-repl "could not find joints node")
269 []))]
270 (assemble-creature model joints)))
272 (def hand "Models/creature1/one.blend")
274 (def worm "Models/creature1/try-again.blend")
276 (def touch "Models/creature1/touch.blend")
278 (defn worm-model [] (load-blender-model worm))
280 (defn x-ray [#^ColorRGBA color]
281 (doto (Material. (asset-manager)
282 "Common/MatDefs/Misc/Unshaded.j3md")
283 (.setColor "Color" color)
284 (-> (.getAdditionalRenderState)
285 (.setDepthTest false))))
287 (defn test-creature [thing]
288 (let [x-axis
289 (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red)
290 y-axis
291 (box 0.01 1 0.01 :physical? false :color ColorRGBA/Green)
292 z-axis
293 (box 0.01 0.01 1 :physical? false :color ColorRGBA/Blue)]
294 (world
295 (nodify [(blender-creature thing)
296 (box 10 2 10 :position (Vector3f. 0 -9 0)
297 :color ColorRGBA/Gray :mass 0)
298 x-axis y-axis z-axis
299 ])
300 standard-debug-controls
301 (fn [world]
302 (light-up-everything world)
303 (enable-debug world)
304 ;;(com.aurellem.capture.Capture/captureVideo
305 ;; world (file-str "/home/r/proj/ai-videos/hand"))
306 (.setTimer world (NanoTimer.))
307 (set-gravity world (Vector3f. 0 0 0))
308 (speed-up world)
309 )
310 no-op
311 ;;(let [timer (atom 0)]
312 ;; (fn [_ _]
313 ;; (swap! timer inc)
314 ;; (if (= (rem @timer 60) 0)
315 ;; (println-repl (float (/ @timer 60))))))
316 )))
319 (defn colorful []
320 (.getChild (worm-model) "worm-21"))
322 (import jme3tools.converters.ImageToAwt)
324 (import ij.ImagePlus)
326 (defn triangle-indices
327 "Get the triangle vertex indices of a given triangle from a given
328 mesh."
329 [#^Mesh mesh triangle-index]
330 (let [indices (int-array 3)]
331 (.getTriangle mesh triangle-index indices)
332 (vec indices)))
334 (defn uv-coord
335 "Get the uv-coordinates of the vertex named by vertex-index"
336 [#^Mesh mesh vertex-index]
337 (let [UV-buffer
338 (.getData
339 (.getBuffer
340 mesh
341 VertexBuffer$Type/TexCoord))]
342 (Vector2f.
343 (.get UV-buffer (* vertex-index 2))
344 (.get UV-buffer (+ 1 (* vertex-index 2))))))
346 (defn tri-uv-coord
347 "Get the uv-cooridnates of the triangle's verticies."
348 [#^Mesh mesh #^Triangle triangle]
349 (map (partial uv-coord mesh)
350 (triangle-indices mesh (.getIndex triangle))))
352 (defn touch-receptor-image
353 "Return the touch-sensor distribution image in ImagePlus format, or
354 nil if it does not exist."
355 [#^Geometry obj]
356 (let [mat (.getMaterial obj)]
357 (if-let [texture-param
358 (.getTextureParam
359 mat
360 MaterialHelper/TEXTURE_TYPE_DIFFUSE)]
361 (let
362 [texture
363 (.getTextureValue texture-param)
364 im (.getImage texture)]
365 (ImagePlus.
366 "UV-map"
367 (ImageToAwt/convert im false false 0))))))
371 (import ij.process.ImageProcessor)
372 (import java.awt.image.BufferedImage)
374 (defprotocol Frame
375 (frame [this]))
377 (extend-type BufferedImage
378 Frame
379 (frame [image]
380 (merge
381 (apply
382 hash-map
383 (interleave
384 (doall (for [x (range (.getWidth image)) y (range (.getHeight image))]
385 (vector x y)))
386 (doall (for [x (range (.getWidth image)) y (range (.getHeight image))]
387 (let [data (.getRGB image x y)]
388 (hash-map :r (bit-shift-right (bit-and 0xff0000 data) 16)
389 :g (bit-shift-right (bit-and 0x00ff00 data) 8)
390 :b (bit-and 0x0000ff data)))))))
391 {:width (.getWidth image) :height (.getHeight image)})))
394 (extend-type ImagePlus
395 Frame
396 (frame [image+]
397 (frame (.getBufferedImage image+))))
400 (def white -1)
402 (defn filter-pixels
403 "List the coordinates of all pixels matching pred."
404 {:author "Dylan Holmes"}
405 [pred #^ImageProcessor ip]
406 (let
407 [width (.getWidth ip)
408 height (.getHeight ip)]
409 ((fn accumulate [x y matches]
410 (cond
411 (>= y height) matches
412 (>= x width) (recur 0 (inc y) matches)
413 (pred (.getPixel ip x y))
414 (recur (inc x) y (conj matches (Vector2f. x y)))
415 :else (recur (inc x) y matches)))
416 0 0 [])))
418 (defn white-coordinates
419 "List the coordinates of all the white pixels in an image."
420 [#^ImageProcessor ip]
421 (filter-pixels #(= % white) ip))
423 (defn same-side? [p1 p2 ref p]
424 (<=
425 0
426 (.dot
427 (.cross (.subtract p2 p1) (.subtract p p1))
428 (.cross (.subtract p2 p1) (.subtract ref p1)))))
431 (defn triangle->matrix4f
432 "Converts the triangle into a 4x4 matrix of vertices: The first
433 three columns contain the vertices of the triangle; the last
434 contains the unit normal of the triangle. The bottom row is filled
435 with 1s."
436 [#^Triangle t]
437 (let [mat (Matrix4f.)
438 [vert-1 vert-2 vert-3]
439 ((comp vec map) #(.get t %) (range 3))
440 unit-normal (do (.calculateNormal t)(.getNormal t))
441 vertices [vert-1 vert-2 vert-3 unit-normal]]
443 (dorun
444 (for [row (range 4) col (range 3)]
445 (do
446 (.set mat col row (.get (vertices row)col))
447 (.set mat 3 row 1))))
448 mat))
450 (defn triangle-transformation
451 "Returns the affine transformation that converts each vertex in the
452 first triangle into the corresponding vertex in the second
453 triangle."
454 [#^Triangle tri-1 #^Triangle tri-2]
455 (.mult
456 (triangle->matrix4f tri-2)
457 (.invert (triangle->matrix4f tri-1))))
459 (def death (Triangle.
460 (Vector3f. 1 1 1)
461 (Vector3f. 1 2 3)
462 (Vector3f. 5 6 7)))
464 (def death-2 (Triangle.
465 (Vector3f. 2 2 2)
466 (Vector3f. 1 1 1)
467 (Vector3f. 0 1 0)))
469 (defn vector2f->vector3f [v]
470 (Vector3f. (.getX v) (.getY v) 0))
473 (extend-type Triangle
474 Textual
475 (text [t]
476 (println "Triangle: " \newline (.get1 t) \newline
477 (.get2 t) \newline (.get3 t))))
480 (defn map-triangle [f #^Triangle tri]
481 (Triangle.
482 (f 0 (.get1 tri))
483 (f 1 (.get2 tri))
484 (f 2 (.get3 tri))))
486 (defn triangle-seq [#^Triangle tri]
487 [(.get1 tri) (.get2 tri) (.get3 tri)])
489 (defn vector3f-seq [#^Vector3f v]
490 [(.getX v) (.getY v) (.getZ v)])
492 (defn inside-triangle?
493 "Is the point inside the triangle? Now what do we do?
494 You might want to hold on there"
495 {:author "Dylan Holmes"}
496 [tri p]
497 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
498 (and
499 (same-side? vert-1 vert-2 vert-3 p)
500 (same-side? vert-2 vert-3 vert-1 p)
501 (same-side? vert-3 vert-1 vert-2 p))))
503 (defn uv-triangle
504 "Convert the mesh triangle into the cooresponding triangle in
505 UV-space. Z-component of these triangles is always zero."
506 [#^Mesh mesh #^Triangle tri]
507 (apply #(Triangle. %1 %2 %3)
508 (map vector2f->vector3f
509 (tri-uv-coord mesh tri))))
511 (defn pixel-triangle
512 "Convert the mesh triange into the corresponding triangle in
513 UV-pixel-space. Z compenent will be zero."
514 [#^Mesh mesh #^Triangle tri width height]
515 (map-triangle (fn [_ v]
516 (Vector3f. (* width (.getX v))
517 (* height (.getY v))
518 0))
519 (uv-triangle mesh tri)))
521 (def rasterize pixel-triangle)
524 (defn triangle-bounds
525 "Dimensions of the bounding square of the triangle in the form
526 [x y width height].
527 Assumes that the triangle lies in the XY plane."
528 [#^Triangle tri]
529 (let [verts (map vector3f-seq (triangle-seq tri))
530 x (apply min (map first verts))
531 y (apply min (map second verts))]
533 [x y
534 (- (apply max (map first verts)) x)
535 (- (apply max (map second verts)) y)
536 ]))
539 (defn locate-feelers
540 "Search the geometry's tactile UV image for touch sensors, returning
541 their positions in geometry-relative coordinates."
542 [#^Geometry geo]
543 (if-let [image (touch-receptor-image geo)]
544 (let [mesh (.getMesh geo)
545 tris (triangles geo)
548 width (.getWidth image)
549 height (.getHeight image)
551 ;; for each triangle
552 sensor-coords
553 (fn [tri]
554 ;; translate triangle to uv-pixel-space
555 (let [uv-tri
556 (pixel-triangle mesh tri width height)
557 bounds (vec (triangle-bounds uv-tri))]
559 ;; get that part of the picture
561 (apply #(.setRoi image %1 %2 %3 %4) bounds)
562 (let [cutout (.crop (.getProcessor image))
563 ;; extract white pixels inside triangle
564 cutout-tri
565 (map-triangle
566 (fn [_ v]
567 (.subtract
568 v
569 (Vector3f. (bounds 0) (bounds 1) (float 0))))
570 uv-tri)
571 whites (filter (partial inside-triangle? cutout-tri)
572 (map vector2f->vector3f
573 (white-coordinates cutout)))
574 ;; translate pixel coordinates to world-space
575 transform (triangle-transformation cutout-tri tri)]
576 (map #(.mult transform %) whites))))]
577 (vec (map sensor-coords tris)))
578 (repeat (count (triangles geo)) [])))
580 (defn enable-touch [#^Geometry geo]
581 (let [feeler-coords (locate-feelers geo)
582 tris (triangles geo)
583 limit 0.1]
584 (fn [node]
585 (let [sensor-origins
586 (map
587 #(map (partial local-to-world geo) %)
588 feeler-coords)
589 triangle-normals
590 (map (partial get-ray-direction geo)
591 tris)
592 rays
593 (flatten
594 (map (fn [origins norm]
595 (map #(doto (Ray. % norm)
596 (.setLimit limit)) origins))
597 sensor-origins triangle-normals))]
598 (for [ray rays]
599 (do
600 (let [results (CollisionResults.)]
601 (.collideWith node ray results)
602 (let [touch-objects
603 (set
604 (filter #(not (= geo %))
605 (map #(.getGeometry %) results)))]
606 (count touch-objects)))))))))
608 (defn touch [#^Node pieces]
609 (let [touch-components
610 (map enable-touch
611 (filter #(isa? (class %) Geometry)
612 (node-seq pieces)))]
613 (fn [node]
614 (reduce into [] (map #(% node) touch-components)))))
621 (defn all-names []
622 (concat
623 (re-split #"\n" (slurp (file-str
624 "/home/r/proj/names/dist.female.first")))
625 (re-split #"\n" (slurp (file-str
626 "/home/r/proj/names/dist.male.first")))
627 (re-split #"\n" (slurp (file-str
628 "/home/r/proj/names/dist.all.last")))))
638 (defrecord LulzLoader [])
639 (defprotocol Lulzable (load-lulz [this]))
640 (extend-type LulzLoader
641 Lulzable
642 (load-lulz [this] (println "the lulz have arrived!")))
645 (defn world-setup [joint]
646 (let [joint-position (Vector3f. 0 0 0)
647 joint-rotation
648 (.toRotationMatrix
649 (.mult
650 (doto (Quaternion.)
651 (.fromAngleAxis
652 (* 1 (/ Math/PI 4))
653 (Vector3f. -1 0 0)))
654 (doto (Quaternion.)
655 (.fromAngleAxis
656 (* 1 (/ Math/PI 2))
657 (Vector3f. 0 0 1)))))
658 top-position (.mult joint-rotation (Vector3f. 8 0 0))
660 origin (doto
661 (sphere 0.1 :physical? false :color ColorRGBA/Cyan
662 :position top-position))
663 top (doto
664 (sphere 0.1 :physical? false :color ColorRGBA/Yellow
665 :position top-position)
667 (.addControl
668 (RigidBodyControl.
669 (CapsuleCollisionShape. 0.5 1.5 1) (float 20))))
670 bottom (doto
671 (sphere 0.1 :physical? false :color ColorRGBA/DarkGray
672 :position (Vector3f. 0 0 0))
673 (.addControl
674 (RigidBodyControl.
675 (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))
676 table (box 10 2 10 :position (Vector3f. 0 -20 0)
677 :color ColorRGBA/Gray :mass 0)
678 a (.getControl top RigidBodyControl)
679 b (.getControl bottom RigidBodyControl)]
681 (cond
682 (= joint :cone)
684 (doto (ConeJoint.
685 a b
686 (world-to-local top joint-position)
687 (world-to-local bottom joint-position)
688 joint-rotation
689 joint-rotation
690 )
693 (.setLimit (* (/ 10) Math/PI)
694 (* (/ 4) Math/PI)
695 0)))
696 [origin top bottom table]))
698 (defn test-joint [joint]
699 (let [[origin top bottom floor] (world-setup joint)
700 control (.getControl top RigidBodyControl)
701 move-up? (atom false)
702 move-down? (atom false)
703 move-left? (atom false)
704 move-right? (atom false)
705 roll-left? (atom false)
706 roll-right? (atom false)
707 timer (atom 0)]
709 (world
710 (nodify [top bottom floor origin])
711 (merge standard-debug-controls
712 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
713 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
714 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
715 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
716 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
717 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
719 (fn [world]
720 (light-up-everything world)
721 (enable-debug world)
722 (set-gravity world (Vector3f. 0 0 0))
723 )
725 (fn [world _]
726 (if (zero? (rem (swap! timer inc) 100))
727 (do
728 ;; (println-repl @timer)
729 (.attachChild (.getRootNode world)
730 (sphere 0.05 :color ColorRGBA/Yellow
731 :position (.getWorldTranslation top)
732 :physical? false))
733 (.attachChild (.getRootNode world)
734 (sphere 0.05 :color ColorRGBA/LightGray
735 :position (.getWorldTranslation bottom)
736 :physical? false))))
738 (if @move-up?
739 (.applyTorque control
740 (.mult (.getPhysicsRotation control)
741 (Vector3f. 0 0 10))))
742 (if @move-down?
743 (.applyTorque control
744 (.mult (.getPhysicsRotation control)
745 (Vector3f. 0 0 -10))))
746 (if @move-left?
747 (.applyTorque control
748 (.mult (.getPhysicsRotation control)
749 (Vector3f. 0 10 0))))
750 (if @move-right?
751 (.applyTorque control
752 (.mult (.getPhysicsRotation control)
753 (Vector3f. 0 -10 0))))
754 (if @roll-left?
755 (.applyTorque control
756 (.mult (.getPhysicsRotation control)
757 (Vector3f. -1 0 0))))
758 (if @roll-right?
759 (.applyTorque control
760 (.mult (.getPhysicsRotation control)
761 (Vector3f. 1 0 0))))))))
764 (defn locate-feelers*
765 "Search the geometry's tactile UV image for touch sensors, returning
766 their positions in geometry-relative coordinates."
767 [#^Geometry geo]
768 (let [uv-image (touch-receptor-image geo)
769 width (.getWidth uv-image)
770 height (.getHeight uv-image)
772 mesh (.getMesh geo)
773 mesh-tris (triangles geo)
775 ;; for each triangle
776 sensor-coords
777 (fn [tri]
778 ;; translate triangle to uv-pixel-space
779 (let [uv-tri
780 (rasterize mesh tri width height)
781 bounds (vec (triangle-bounds uv-tri))]
783 ;; get that part of the picture
785 (apply (partial (memfn setRoi) uv-image) bounds)
786 (let [cutout (.crop (.getProcessor uv-image))
787 ;; extract white pixels inside triangle
788 cutout-tri
789 (map-triangle
790 (fn [_ v]
791 (.subtract
792 v
793 (Vector3f. (bounds 0) (bounds 1) (float 0))))
794 uv-tri)
795 whites (filter (partial inside-triangle? cutout-tri)
796 (map vector2f->vector3f
797 (white-coordinates cutout)))
798 ;; translate pixel coordinates to world-space
799 transform (triangle-transformation cutout-tri tri)]
800 (map #(.mult transform %) whites))))]
804 (for [mesh-tri mesh-tris]
806 (let [uv-tri (rasterize mesh mesh-tri width height)
807 bounding-box (vec (triangle-bounds uv-tri))]
808 (apply (partial (memfn setRoi) uv-image) bounding-box)
809 ))
810 (vec (map sensor-coords mesh-tris))))
813 (defn tactile-coords [#^Geometry obj]
814 (let [mesh (.getMesh obj)
815 num-triangles (.getTriangleCount mesh)
816 num-verticies (.getVertexCount mesh)
817 uv-coord (partial uv-coord mesh)
818 triangle-indices (partial triangle-indices mesh)
819 receptors (touch-receptor-image obj)
820 tris (triangles obj)
821 ]
822 (map
823 (fn [[tri-1 tri-2 tri-3]]
824 (let [width (.getWidth receptors)
825 height (.getHeight receptors)
826 uv-1 (uv-coord tri-1)
827 uv-2 (uv-coord tri-2)
828 uv-3 (uv-coord tri-3)
829 x-coords (map #(.getX %) [uv-1 uv-2 uv-3])
830 y-coords (map #(.getY %) [uv-1 uv-2 uv-3])
831 max-x (Math/ceil (* width (apply max x-coords)))
832 min-x (Math/floor (* width (apply min x-coords)))
833 max-y (Math/ceil (* height (apply max y-coords)))
834 min-y (Math/floor (* height (apply min y-coords)))
836 image-1 (Vector2f. (* width (.getX uv-1))
837 (* height (.getY uv-1)))
838 image-2 (Vector2f. (* width (.getX uv-2))
839 (* height (.getY uv-2)))
840 image-3 (Vector2f. (* width (.getX uv-3))
841 (* height (.getY uv-3)))
842 left-corner
843 (Vector2f. min-x min-y)
844 ]
846 (.setRoi receptors min-x min-y (- max-x min-x) (- max-y min-y))
847 (let [processor (.crop (.getProcessor receptors))]
848 (map
849 #(.add left-corner %)
851 (filter
852 (partial
853 inside-triangle?
854 (.subtract image-1 left-corner)
855 (.subtract image-2 left-corner)
856 (.subtract image-3 left-corner))
857 (white-coordinates processor))))
858 )) (map triangle-indices (range num-triangles)))))
860 #+end_src
862 #+results: body-1
863 : #'cortex.silly/test-joint
866 * COMMENT purgatory
867 #+begin_src clojure
868 (defn bullet-trans []
869 (let [obj-a (sphere 0.5 :color ColorRGBA/Red
870 :position (Vector3f. -10 5 0))
871 obj-b (sphere 0.5 :color ColorRGBA/Blue
872 :position (Vector3f. -10 -5 0)
873 :mass 0)
874 control-a (.getControl obj-a RigidBodyControl)
875 control-b (.getControl obj-b RigidBodyControl)
876 swivel
877 (.toRotationMatrix
878 (doto (Quaternion.)
879 (.fromAngleAxis (/ Math/PI 2)
880 Vector3f/UNIT_X)))]
881 (doto
882 (ConeJoint.
883 control-a control-b
884 (Vector3f. 0 5 0)
885 (Vector3f. 0 -5 0)
886 swivel swivel)
887 (.setLimit (* 0.6 (/ Math/PI 4))
888 (/ Math/PI 4)
889 (* Math/PI 0.8)))
890 (world (nodify
891 [obj-a obj-b])
892 standard-debug-controls
893 enable-debug
894 no-op)))
897 (defn bullet-trans* []
898 (let [obj-a (box 1.5 0.5 0.5 :color ColorRGBA/Red
899 :position (Vector3f. 5 0 0)
900 :mass 90)
901 obj-b (sphere 0.5 :color ColorRGBA/Blue
902 :position (Vector3f. -5 0 0)
903 :mass 0)
904 control-a (.getControl obj-a RigidBodyControl)
905 control-b (.getControl obj-b RigidBodyControl)
906 move-up? (atom nil)
907 move-down? (atom nil)
908 move-left? (atom nil)
909 move-right? (atom nil)
910 roll-left? (atom nil)
911 roll-right? (atom nil)
912 force 100
913 swivel
914 (.toRotationMatrix
915 (doto (Quaternion.)
916 (.fromAngleAxis (/ Math/PI 2)
917 Vector3f/UNIT_X)))
918 x-move
919 (doto (Matrix3f.)
920 (.fromStartEndVectors Vector3f/UNIT_X
921 (.normalize (Vector3f. 1 1 0))))
923 timer (atom 0)]
924 (doto
925 (ConeJoint.
926 control-a control-b
927 (Vector3f. -8 0 0)
928 (Vector3f. 2 0 0)
929 ;;swivel swivel
930 ;;Matrix3f/IDENTITY Matrix3f/IDENTITY
931 x-move Matrix3f/IDENTITY
932 )
933 (.setCollisionBetweenLinkedBodys false)
934 (.setLimit (* 1 (/ Math/PI 4)) ;; twist
935 (* 1 (/ Math/PI 4)) ;; swing span in X-Y plane
936 (* 0 (/ Math/PI 4)))) ;; swing span in Y-Z plane
937 (world (nodify
938 [obj-a obj-b])
939 (merge standard-debug-controls
940 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
941 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
942 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
943 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
944 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
945 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
947 (fn [world]
948 (enable-debug world)
949 (set-gravity world Vector3f/ZERO)
950 )
952 (fn [world _]
954 (if @move-up?
955 (.applyForce control-a
956 (Vector3f. force 0 0)
957 (Vector3f. 0 0 0)))
958 (if @move-down?
959 (.applyForce control-a
960 (Vector3f. (- force) 0 0)
961 (Vector3f. 0 0 0)))
962 (if @move-left?
963 (.applyForce control-a
964 (Vector3f. 0 force 0)
965 (Vector3f. 0 0 0)))
966 (if @move-right?
967 (.applyForce control-a
968 (Vector3f. 0 (- force) 0)
969 (Vector3f. 0 0 0)))
971 (if @roll-left?
972 (.applyForce control-a
973 (Vector3f. 0 0 force)
974 (Vector3f. 0 0 0)))
975 (if @roll-right?
976 (.applyForce control-a
977 (Vector3f. 0 0 (- force))
978 (Vector3f. 0 0 0)))
980 (if (zero? (rem (swap! timer inc) 100))
981 (.attachChild
982 (.getRootNode world)
983 (sphere 0.05 :color ColorRGBA/Yellow
984 :physical? false :position
985 (.getWorldTranslation obj-a)))))
986 )
987 ))
991 #+end_src
994 * COMMENT generate source
995 #+begin_src clojure :tangle ../src/cortex/silly.clj
996 <<body-1>>
997 #+end_src
1003 (defn transform-trianglesdsd
1004 "Transform that converts each vertex in the first triangle
1005 into the corresponding vertex in the second triangle."
1006 [#^Triangle tri-1 #^Triangle tri-2]
1007 (let [in [(.get1 tri-1)
1008 (.get2 tri-1)
1009 (.get3 tri-1)]
1010 out [(.get1 tri-2)
1011 (.get2 tri-2)
1012 (.get3 tri-2)]]
1013 (let [translate (doto (Matrix4f.) (.setTranslation (.negate (in 0))))
1014 in* [(.mult translate (in 0))
1015 (.mult translate (in 1))
1016 (.mult translate (in 2))]
1017 final-translation
1018 (doto (Matrix4f.)
1019 (.setTranslation (out 1)))
1021 rotate-1
1022 (doto (Matrix3f.)
1023 (.fromStartEndVectors
1024 (.normalize
1025 (.subtract
1026 (in* 1) (in* 0)))
1027 (.normalize
1028 (.subtract
1029 (out 1) (out 0)))))
1030 in** [(.mult rotate-1 (in* 0))
1031 (.mult rotate-1 (in* 1))
1032 (.mult rotate-1 (in* 2))]
1033 scale-factor-1
1034 (.mult
1035 (.normalize
1036 (.subtract
1037 (out 1)
1038 (out 0)))
1039 (/ (.length
1040 (.subtract (out 1)
1041 (out 0)))
1042 (.length
1043 (.subtract (in** 1)
1044 (in** 0)))))
1045 scale-1 (doto (Matrix4f.) (.setScale scale-factor-1))
1046 in*** [(.mult scale-1 (in** 0))
1047 (.mult scale-1 (in** 1))
1048 (.mult scale-1 (in** 2))]
1056 (dorun (map println in))
1057 (println)
1058 (dorun (map println in*))
1059 (println)
1060 (dorun (map println in**))
1061 (println)
1062 (dorun (map println in***))
1063 (println)
1065 )))