#+title: First attempt at a creature!

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: 

 #+keywords: simulation, jMonkeyEngine3, clojure

 #+SETUPFILE: ../../aurellem/org/setup.org

 #+INCLUDE: ../../aurellem/org/level-0.org

 

 

 * objectives

  - [ ] get an overall bitmap-like image for touch

  - [ ] write code to visuliaze this bitmap

  - [ ] directly change the UV-pixels to show touch sensor activation

  - [ ] write an explination for why b&w bitmaps for senses is appropiate

  - [ ] clean up touch code and write visulazation test

  - [ ] do the same for eyes

 

 * Intro 

 So far, I've made the following senses -- 

  - Vision

  - Hearing

  - Touch

  - Proprioception

 

 And one effector: 

  - Movement

 

 However, the code so far has only enabled these senses, but has not

 actually implemented them.  For example, there is still a lot of work

 to be done for vision. I need to be able to create an /eyeball/ in

 simulation that can be moved around and see the world from different

 angles. I also need to determine weather to use log-polar or cartesian

 for the visual input, and I need to determine how/wether to

 disceritise the visual input.

 

 I also want to be able to visualize both the sensors and the

 effectors in pretty pictures. This semi-retarted creature will by my

 first attempt at bringing everything together.

 

 * The creature's body

 

 Still going to do an eve-like body in blender, but due to problems

 importing the joints, etc into jMonkeyEngine3, I',m going to do all

 the connecting here in clojure code, using the names of the individual

 components and trial and error. Later, I'll maybe make some sort of

 creature-building modifications to blender that support whatever

 discreitized senses I'm going to make.

 

 #+name: body-1

 #+begin_src clojure 

 (ns cortex.silly

   "let's play!"

   {:author "Robert McIntyre"})

 

 ;; TODO remove this!

 (require 'cortex.import)

 (cortex.import/mega-import-jme3)

 (use '(cortex world util body hearing touch vision))

 

 (rlm.rlm-commands/help)

 (import java.awt.image.BufferedImage)

 (import javax.swing.JPanel)

 (import javax.swing.SwingUtilities)

 (import java.awt.Dimension)

 (import javax.swing.JFrame)

 (import java.awt.Dimension)

 (declare joint-create)

 

 (defn view-image

   "Initailizes a JPanel on which you may draw a BufferedImage.

    Returns a function that accepts a BufferedImage and draws it to the

    JPanel."

   []

   (let [image

         (atom

          (BufferedImage. 1 1 BufferedImage/TYPE_4BYTE_ABGR))

         panel 

         (proxy [JPanel] []

           (paint

             [graphics]

             (proxy-super paintComponent graphics)

             (.drawImage graphics @image 0 0 nil)))

         frame (JFrame. "Display Image")]

     (SwingUtilities/invokeLater

      (fn []

        (doto frame

          (-> (.getContentPane) (.add panel))

          (.pack)

          (.setLocationRelativeTo nil)

          (.setResizable true)

          (.setVisible true))))

     (fn [#^BufferedImage i]

       (reset! image i)

       (.setSize frame (+ 8 (.getWidth i)) (+ 28 (.getHeight i)))

       (.repaint panel 0 0 (.getWidth i) (.getHeight i)))))

 

 (defn points->image

   "Take a sparse collection of points and visuliaze it as a

    BufferedImage."

 

   ;; TODO maybe parallelize this since it's easy

 

   [points]

   (let [xs (vec (map first points))

         ys (vec (map second points))

         x0 (apply min xs)

         y0 (apply min ys)

         width (- (apply max xs) x0)

         height (- (apply max ys) y0)

         image (BufferedImage. (inc width) (inc height)

                               BufferedImage/TYPE_BYTE_BINARY)]

     (dorun 

      (for [index (range (count points))]

        (.setRGB image (- (xs index) x0) (- (ys index) y0) -1)))

 

     image))

 

 (defn test-data

   []

   (vec

    (for [a (range 0 1000 2)

          b (range 0 1000 2)]

      (vector a b))

   ))

 

 (defn average [coll]

   (/ (reduce + coll) (count coll)))

 

 (defn collapse-1d

   "One dimensional analogue of collapse"

   [center line]

   (let [length (count line)

         num-above (count (filter (partial < center) line))

         num-below (- length num-above)]

     (range (- center num-below)

            (+ center num-above))

     ))

 

 (defn collapse

   "Take a set of pairs of integers and collapse them into a

    contigous bitmap."

   [points]

   (let

       [num-points (count points)

        center (vector

                (int (average (map first points)))

                (int (average (map first points))))

        flattened

        (reduce

         concat 

         (map

          (fn [column]

            (map vector

                 (map first column)

                 (collapse-1d (second center)

                              (map second column))))

          (partition-by first (sort-by first points))))

        squeezed

        (reduce

         concat 

         (map

          (fn [row]

            (map vector

                 (collapse-1d (first center)

                              (map first row))

                 (map second row)))

          (partition-by second (sort-by second flattened))))

        ;;vi (view-image)

        relocate

        (let [min-x (apply min (map first squeezed))

              min-y (apply min (map second squeezed))]

          (map (fn [[x y]]

                 [(- x min-x)

                  (- y min-y)])

               squeezed))

        ]

     ;;(vi (points->image points))

     ;;(Thread/sleep 1000)

     ;;(vi (points->image flattened))

     ;;(Thread/sleep 1000)

     ;;(vi (points->image squeezed))

     relocate

    ))

 

 (defn load-bullet []

   (let [sim (world (Node.) {} no-op no-op)]

     (doto sim

       (.enqueue

        (fn []

          (.stop sim)))

       (.start))))

 

 (defn load-blender-model

   "Load a .blend file using an asset folder relative path."

   [^String model]

   (.loadModel

    (doto (asset-manager)

      (.registerLoader BlenderModelLoader (into-array String ["blend"])))

    model))

 

 (defn meta-data [blender-node key]

   (if-let [data (.getUserData blender-node "properties")]

     (.findValue data key)

     nil))

 

 (defn blender-to-jme

   "Convert from Blender coordinates to JME coordinates"

   [#^Vector3f in]

   (Vector3f. (.getX in)

              (.getZ in)

              (- (.getY in))))

 

 (defn jme-to-blender

   "Convert from JME coordinates to Blender coordinates"

   [#^Vector3f in]

   (Vector3f. (.getX in)

              (- (.getZ in))

              (.getY in)))

 

 (defn joint-targets

   "Return the two closest two objects to the joint object, ordered

   from bottom to top according to the joint's rotation."

   [#^Node parts #^Node joint]

   ;;(println (meta-data joint "joint"))

   (.getWorldRotation joint)

   (loop [radius (float 0.01)]

     (let [results (CollisionResults.)]

       (.collideWith

        parts

        (BoundingBox. (.getWorldTranslation joint)

                      radius radius radius)

        results)

       (let [targets

             (distinct

              (map  #(.getGeometry %) results))]

         (if (>= (count targets) 2)

           (sort-by

            #(let [v

                   (jme-to-blender

                    (.mult

                     (.inverse (.getWorldRotation joint))

                     (.subtract (.getWorldTranslation %)

                                (.getWorldTranslation joint))))]

               (println-repl (.getName %) ":" v)

               (.dot (Vector3f. 1 1 1)

                     v))                  

            (take 2 targets))

           (recur (float (* radius 2))))))))

 

 (defn world-to-local

    "Convert the world coordinates into coordinates relative to the 

    object (i.e. local coordinates), taking into account the rotation

    of object."

    [#^Spatial object world-coordinate]

    (let [out (Vector3f.)]

      (.worldToLocal object world-coordinate out) out))

 

 (defn local-to-world

    "Convert the local coordinates into coordinates into world relative

     coordinates" 

    [#^Spatial object local-coordinate]

    (let [world-coordinate (Vector3f.)]

      (.localToWorld object local-coordinate world-coordinate)

      world-coordinate))

 

 

 (defmulti joint-dispatch

   "Translate blender pseudo-joints into real JME joints."

   (fn [constraints & _] 

     (:type constraints)))

 

 (defmethod joint-dispatch :point

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating POINT2POINT joint")

   (Point2PointJoint.

    control-a

    control-b

    pivot-a

    pivot-b))

 

 (defmethod joint-dispatch :hinge

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating HINGE joint")

   (let [axis

         (if-let

             [axis (:axis constraints)]

           axis

           Vector3f/UNIT_X)

         [limit-1 limit-2] (:limit constraints)

         hinge-axis

         (.mult

          rotation

          (blender-to-jme axis))]

     (doto

         (HingeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          hinge-axis

          hinge-axis)

       (.setLimit limit-1 limit-2))))

 

 (defmethod joint-dispatch :cone

   [constraints control-a control-b pivot-a pivot-b rotation]

   (let [limit-xz (:limit-xz constraints)

         limit-xy (:limit-xy constraints)

         twist    (:twist constraints)]

     

     (println-repl "creating CONE joint")

     (println-repl rotation)

     (println-repl

      "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))

     (println-repl

      "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))

     (println-repl

      "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))

     (doto

         (ConeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          rotation

          rotation)

       (.setLimit (float limit-xz)

                  (float limit-xy)

                  (float twist)))))

 

 (defn connect

   "here are some examples:

    {:type :point}

    {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}

    (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)

 

    {:type :cone :limit-xz 0]

                 :limit-xy 0]

                 :twist 0]}   (use XZY rotation mode in blender!)"

   [#^Node obj-a #^Node obj-b #^Node joint]

   (let [control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         joint-center (.getWorldTranslation joint)

         joint-rotation (.toRotationMatrix (.getWorldRotation joint))

         pivot-a (world-to-local obj-a joint-center)

         pivot-b (world-to-local obj-b joint-center)]

    

     (if-let [constraints

              (map-vals

               eval

               (read-string

                (meta-data joint "joint")))]

       ;; A side-effect of creating a joint registers

       ;; it with both physics objects which in turn

       ;; will register the joint with the physics system

       ;; when the simulation is started.

       (do

         (println-repl "creating joint between"

                       (.getName obj-a) "and" (.getName obj-b))

         (joint-dispatch constraints

                         control-a control-b

                         pivot-a pivot-b

                         joint-rotation))

       (println-repl "could not find joint meta-data!"))))

 

 (defn assemble-creature [#^Node pieces joints]

   (dorun

    (map

     (fn [geom]

       (let [physics-control

             (RigidBodyControl.

              (HullCollisionShape.

               (.getMesh geom))

              (if-let [mass (meta-data geom "mass")]

                (do

                  (println-repl

                   "setting" (.getName geom) "mass to" (float mass))

                  (float mass))

                (float 1)))]

                

         (.addControl geom physics-control)))

     (filter #(isa? (class %) Geometry )

             (node-seq pieces))))

   (dorun

    (map

     (fn [joint]

       (let [[obj-a obj-b]

             (joint-targets pieces joint)]

         (connect obj-a obj-b joint)))

     joints))

   pieces)

 

 (defn blender-creature [blender-path]

   (let [model (load-blender-model blender-path)

         joints

         (if-let [joint-node (.getChild model "joints")]

           (seq (.getChildren joint-node))

           (do (println-repl "could not find joints node")

               []))]

   (assemble-creature model joints)))

 

 (def hand "Models/creature1/one.blend")

 

 (def worm "Models/creature1/try-again.blend")

 

 (def touch "Models/creature1/touch.blend")

 

 (defn worm-model [] (load-blender-model worm))

 

 (defn x-ray [#^ColorRGBA color]

   (doto (Material. (asset-manager)

                    "Common/MatDefs/Misc/Unshaded.j3md")

     (.setColor "Color" color)

     (-> (.getAdditionalRenderState)

         (.setDepthTest false))))

 

 (defn test-creature [thing]

   (let [x-axis

         (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red)

         y-axis

         (box 0.01 1 0.01 :physical? false :color ColorRGBA/Green)

         z-axis

         (box 0.01 0.01 1 :physical? false :color ColorRGBA/Blue)]

   (world

    (nodify [(blender-creature thing)

             (box 10 2 10 :position (Vector3f. 0 -9 0)

                  :color ColorRGBA/Gray :mass 0)

             x-axis y-axis z-axis

             ])

    standard-debug-controls

    (fn [world]

      (light-up-everything world)

      (enable-debug world)

      ;;(com.aurellem.capture.Capture/captureVideo

      ;; world (file-str "/home/r/proj/ai-videos/hand"))

      (.setTimer world (NanoTimer.))

      (set-gravity world (Vector3f. 0 0 0))

      (speed-up world)

      )

    no-op

    ;;(let [timer (atom 0)]

    ;;  (fn [_ _]

    ;;    (swap! timer inc)

    ;;    (if (= (rem @timer 60) 0)

    ;;      (println-repl (float (/ @timer 60))))))

    )))

 

 (defn colorful []

   (.getChild (worm-model) "worm-21"))

 

 (import jme3tools.converters.ImageToAwt)

 

 (import ij.ImagePlus)

 

 (defn triangle-indices

   "Get the triangle vertex indices of a given triangle from a given

    mesh."

   [#^Mesh mesh triangle-index]

   (let [indices (int-array 3)]

     (.getTriangle mesh triangle-index indices)

     (vec indices)))

 

 (defn uv-coord

   "Get the uv-coordinates of the vertex named by vertex-index"

   [#^Mesh mesh vertex-index]

   (let [UV-buffer

         (.getData

          (.getBuffer

           mesh

           VertexBuffer$Type/TexCoord))]

     (Vector2f.

      (.get UV-buffer (* vertex-index 2))

      (.get UV-buffer (+ 1 (* vertex-index 2))))))

 

 (defn tri-uv-coord

   "Get the uv-cooridnates of the triangle's verticies."

   [#^Mesh mesh #^Triangle triangle]

   (map (partial uv-coord mesh)

        (triangle-indices mesh (.getIndex triangle))))

 

 (defn touch-receptor-image

   "Return the touch-sensor distribution image in ImagePlus format, or

    nil if it does not exist."

   [#^Geometry obj]

   (let [mat (.getMaterial obj)]

     (if-let [texture-param

              (.getTextureParam

               mat

               MaterialHelper/TEXTURE_TYPE_DIFFUSE)]

       (let

           [texture

            (.getTextureValue texture-param)

            im (.getImage texture)]

         (ImagePlus.

          "UV-map"

          (ImageToAwt/convert im false false 0))))))

 

 (import ij.process.ImageProcessor)

 (import java.awt.image.BufferedImage)

 

 (defprotocol Frame

   (frame [this]))

 

 (extend-type BufferedImage

   Frame

   (frame [image]

     (merge  

      (apply

       hash-map 

       (interleave 

        (doall (for [x (range (.getWidth image)) y (range (.getHeight image))]

                 (vector x y)))

        (doall (for [x (range (.getWidth image)) y (range (.getHeight image))]

                 (let [data (.getRGB image x y)] 

                   (hash-map :r (bit-shift-right (bit-and 0xff0000 data) 16)

                             :g (bit-shift-right (bit-and 0x00ff00 data) 8)

                             :b (bit-and 0x0000ff data)))))))

      {:width  (.getWidth image) :height (.getHeight image)})))

 

 

 (extend-type ImagePlus

   Frame

   (frame [image+]

     (frame (.getBufferedImage image+))))

 

 

 (def white -1)

   

 (defn filter-pixels

   "List the coordinates of all pixels matching pred."

   {:author "Dylan Holmes"}

   [pred #^ImageProcessor ip]

   (let

       [width (.getWidth ip)

        height (.getHeight ip)]

     ((fn accumulate [x y matches]

        (cond

         (>= y height) matches

         (>= x width) (recur 0 (inc y) matches)

         (pred (.getPixel ip x y))

         (recur (inc x) y (conj matches (Vector2f. x y)))

         :else (recur (inc x) y matches)))

      0 0 [])))

 

 (defn white-coordinates

   "List the coordinates of all the white pixels in an image."

   [#^ImageProcessor ip]

   (filter-pixels #(= % white) ip))

   

 (defn same-side?

   "Given the points p1 and p2 and the reference point ref, is point p

   on the same side of the line that goes through p1 and p2 as ref is?" 

   [p1 p2 ref p]

   (<=

    0

    (.dot 

     (.cross (.subtract p2 p1) (.subtract p p1))

     (.cross (.subtract p2 p1) (.subtract ref p1)))))

 

 (defn triangle->matrix4f

   "Converts the triangle into a 4x4 matrix of vertices: The first

    three columns contain the vertices of the triangle; the last

    contains the unit normal of the triangle. The bottom row is filled

    with 1s."

   [#^Triangle t]

   (let [mat (Matrix4f.)

         [vert-1 vert-2 vert-3]

         ((comp vec map) #(.get t %) (range 3))

         unit-normal (do (.calculateNormal t)(.getNormal t))

         vertices [vert-1 vert-2 vert-3 unit-normal]]

     (dorun 

      (for [row (range 4) col (range 3)]

        (do

          (.set mat col row (.get (vertices row)col))

          (.set mat 3 row 1))))

     mat))

 

 (defn triangle-transformation

   "Returns the affine transformation that converts each vertex in the

    first triangle into the corresponding vertex in the second

    triangle."

   [#^Triangle tri-1 #^Triangle tri-2]

   (.mult 

    (triangle->matrix4f tri-2)

    (.invert (triangle->matrix4f tri-1))))

 

 (def death (Triangle. 

             (Vector3f. 1 1 1)

             (Vector3f. 1 2 3)

             (Vector3f. 5 6 7)))

 

 (def death-2 (Triangle. 

              (Vector3f. 2 2 2)

              (Vector3f. 1 1 1)

              (Vector3f. 0 1 0)))

 

 (defn vector2f->vector3f [v]

   (Vector3f. (.getX v) (.getY v) 0))

 

 (extend-type Triangle

   Textual

   (text [t]

     (println "Triangle: " \newline (.get1 t) \newline

              (.get2 t) \newline (.get3 t))))

 

 (defn map-triangle [f #^Triangle tri]

   (Triangle.

    (f 0 (.get1 tri))

    (f 1 (.get2 tri))

    (f 2 (.get3 tri))))

 

 (defn triangle-seq [#^Triangle tri]

   [(.get1 tri) (.get2 tri) (.get3 tri)])

 

 (defn vector3f-seq [#^Vector3f v]

   [(.getX v) (.getY v) (.getZ v)])

 

 (defn inside-triangle?

   "Is the point inside the triangle? Now what do we do?

    You might want to hold on there"

   {:author "Dylan Holmes"}

   [tri p]

   (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]

     (and

      (same-side? vert-1 vert-2 vert-3 p)

      (same-side? vert-2 vert-3 vert-1 p)

      (same-side? vert-3 vert-1 vert-2 p))))

 

 (defn uv-triangle

   "Convert the mesh triangle into the cooresponding triangle in

    UV-space. Z-component of these triangles is always zero." 

   [#^Mesh mesh #^Triangle tri]

   (apply #(Triangle. %1 %2 %3)

          (map vector2f->vector3f

               (tri-uv-coord mesh tri))))

 

 (defn pixel-triangle

   "Convert the mesh triangle into the corresponding triangle in

   UV-pixel-space. Z compenent will be zero."

   [#^Mesh mesh #^Triangle tri width height]

   (map-triangle (fn [_ v]

                   (Vector3f. (* width (.getX v))

                              (* height (.getY v))

                              0))

                 (uv-triangle mesh tri)))

 

 (def rasterize pixel-triangle)

 

 

 (defn triangle-bounds

   "Dimensions of the bounding square of the triangle in the form

    [x y width height].

    Assumes that the triangle lies in the XY plane."

   [#^Triangle tri]

   (let [verts (map vector3f-seq (triangle-seq tri))

         x (apply min (map first verts))

         y (apply min (map second verts))]

     [x y 

      (- (apply max (map first verts)) x)

      (- (apply max (map second verts)) y)

      ]))

   

 

 (defn locate-feelers

   "Search the geometry's tactile UV image for touch sensors, returning

   their positions in geometry-relative coordinates."

   [#^Geometry geo]

   (if-let [image (touch-receptor-image geo)]

     (let [mesh (.getMesh geo)

           tris (triangles geo)

 

           width (.getWidth image)

           height (.getHeight image)

           

           ;; for each triangle

           sensor-coords

           (fn [tri]

             ;; translate triangle to uv-pixel-space

             (let [uv-tri

                   (pixel-triangle mesh tri width height)

                   bounds (vec (triangle-bounds uv-tri))]

               

               ;; get that part of the picture

               

               (apply #(.setRoi image %1 %2 %3 %4) bounds)

               (let [cutout (.crop (.getProcessor image))

                     ;; extract white pixels inside triangle

                     cutout-tri

                     (map-triangle

                      (fn [_ v]

                        (.subtract

                         v

                         (Vector3f. (bounds 0) (bounds 1) (float 0))))

                      uv-tri)

                     whites (filter (partial inside-triangle? cutout-tri)

                                    (map vector2f->vector3f

                                         (white-coordinates cutout)))

                     ;; translate pixel coordinates to world-space

                     transform (triangle-transformation cutout-tri tri)]

                 (map #(.mult transform %) whites))))]

       (vec (map sensor-coords tris)))

     (repeat (count (triangles geo)) [])))

 

 (use 'clojure.contrib.def)

 

 (defn-memo touch-topology [#^Gemoetry geo]

   (let [feeler-coords

         (map

          #(vector (int (.getX %)) (int (.getY %)))

          (white-coordinates

           (.getProcessor (touch-receptor-image (colorful)))))]

     (vec (collapse feeler-coords))))

 

 (defn enable-touch [#^Geometry geo]

   (let [feeler-coords (locate-feelers geo)

         tris (triangles geo)

         limit 0.1]

     (fn [node]

       (let [sensor-origins 

             (map

              #(map (partial local-to-world geo) %)

              feeler-coords)

             triangle-normals 

             (map (partial get-ray-direction geo)

                  tris)

             rays

              (flatten

               (map (fn [origins norm]

                      (map #(doto (Ray. % norm)

                               (.setLimit limit)) origins))

                    sensor-origins triangle-normals))]

         (vector

          (touch-topology geo)

          (vec

           (for [ray rays]

             (do

               (let [results (CollisionResults.)]

                 (.collideWith node ray results)

                 (let [touch-objects

                       (set

                        (filter #(not (= geo %))

                                (map #(.getGeometry %) results)))]

                   (if (> (count touch-objects) 0)

                     1 0)))))))))))

 

 (defn touch [#^Node pieces]

   (map enable-touch

        (filter #(isa? (class %) Geometry)

                (node-seq pieces))))

 

     

 (defn all-names []

   (concat 

    (re-split #"\n" (slurp (file-str

                            "/home/r/proj/names/dist.female.first")))

    (re-split #"\n" (slurp (file-str

                            "/home/r/proj/names/dist.male.first")))

    (re-split #"\n" (slurp (file-str

                            "/home/r/proj/names/dist.all.last")))))

 

 

 

 

 

 

 

 

        

 (defrecord LulzLoader [])

 (defprotocol Lulzable (load-lulz [this]))

 (extend-type LulzLoader

   Lulzable

   (load-lulz [this] (println "the lulz have arrived!")))

 

 

 (defn world-setup [joint]

   (let [joint-position (Vector3f. 0 0 0)

         joint-rotation

         (.toRotationMatrix

          (.mult

           (doto (Quaternion.)

             (.fromAngleAxis

              (* 1 (/ Math/PI 4))

              (Vector3f. -1 0 0)))

           (doto (Quaternion.)

             (.fromAngleAxis

              (* 1 (/ Math/PI 2))

              (Vector3f. 0 0 1)))))

         top-position (.mult joint-rotation (Vector3f. 8 0 0))

         

         origin (doto

                    (sphere 0.1 :physical? false :color ColorRGBA/Cyan

                            :position top-position))

         top (doto

                 (sphere 0.1 :physical? false :color ColorRGBA/Yellow

                         :position top-position)

 

               (.addControl

                (RigidBodyControl.

                 (CapsuleCollisionShape. 0.5 1.5 1) (float 20))))

         bottom (doto

                    (sphere 0.1 :physical? false :color ColorRGBA/DarkGray

                            :position (Vector3f. 0 0 0))

                  (.addControl

                (RigidBodyControl.

                 (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))

         table (box 10 2 10 :position (Vector3f. 0 -20 0)

                    :color ColorRGBA/Gray :mass 0)

         a (.getControl top RigidBodyControl)

         b (.getControl bottom RigidBodyControl)]

 

     (cond

      (= joint :cone)

      

        (doto (ConeJoint.

               a b

               (world-to-local top joint-position)

               (world-to-local bottom joint-position)

               joint-rotation

               joint-rotation

               )

 

               

          (.setLimit (* (/ 10) Math/PI)

                     (* (/ 4)  Math/PI)

                     0)))

   [origin top bottom table]))

             

 (defn test-joint [joint]

   (let [[origin top bottom floor] (world-setup joint)

         control (.getControl top RigidBodyControl)

         move-up? (atom false)

         move-down? (atom false)

         move-left? (atom false)

         move-right? (atom false)

         roll-left? (atom false)

         roll-right? (atom false)

         timer (atom 0)]

 

     (world

      (nodify [top bottom floor origin])

      (merge standard-debug-controls

             {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))

              "key-t" (fn [_ pressed?] (reset! move-down? pressed?))

              "key-f" (fn [_ pressed?] (reset! move-left? pressed?))

              "key-g" (fn [_ pressed?] (reset! move-right? pressed?))

              "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))

              "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     

      

      (fn [world]

        (light-up-everything world)

        (enable-debug world)

        (set-gravity world (Vector3f. 0 0 0))

        )

      

      (fn [world _]

        (if (zero? (rem (swap! timer inc) 100))

          (do

          ;;  (println-repl @timer)

            (.attachChild (.getRootNode world)

                        (sphere 0.05 :color ColorRGBA/Yellow

                                :position (.getWorldTranslation top)

                                :physical? false))

            (.attachChild (.getRootNode world)

                          (sphere 0.05 :color ColorRGBA/LightGray

                                  :position (.getWorldTranslation bottom)

                                  :physical? false))))

 

        (if @move-up?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 0 10))))

        (if @move-down?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 0 -10))))

        (if @move-left?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 10 0))))

        (if @move-right?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 -10 0))))

        (if @roll-left?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. -1 0 0))))

        (if @roll-right?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 1 0 0))))))))

 

 

 (defn locate-feelers*

   "Search the geometry's tactile UV image for touch sensors, returning

   their positions in geometry-relative coordinates."

   [#^Geometry geo]

   (let [uv-image (touch-receptor-image geo)

         width (.getWidth uv-image)

         height (.getHeight uv-image)

 

         mesh (.getMesh geo)

         mesh-tris (triangles geo)

 

         ;; for each triangle

         sensor-coords

         (fn [tri]

           ;; translate triangle to uv-pixel-space

           (let [uv-tri

                 (rasterize mesh tri width height)

                 bounds (vec (triangle-bounds uv-tri))]

             

             ;; get that part of the picture

             

             (apply (partial (memfn setRoi) uv-image) bounds)

             (let [cutout (.crop (.getProcessor uv-image))

                   ;; extract white pixels inside triangle

                   cutout-tri

                   (map-triangle

                    (fn [_ v]

                      (.subtract

                       v

                       (Vector3f. (bounds 0) (bounds 1) (float 0))))

                    uv-tri)

                   whites (filter (partial inside-triangle? cutout-tri)

                                  (map vector2f->vector3f

                                       (white-coordinates cutout)))

                   ;; translate pixel coordinates to world-space

                   transform (triangle-transformation cutout-tri tri)]

               (map #(.mult transform %) whites))))]

      

     (for [mesh-tri mesh-tris]

      

       (let [uv-tri (rasterize mesh mesh-tri width height)

             bounding-box (vec (triangle-bounds uv-tri))]

         (apply (partial (memfn setRoi) uv-image) bounding-box)

         ))   

     (vec (map sensor-coords mesh-tris))))

 

 

 (defn tactile-coords [#^Geometry obj]

   (let [mesh (.getMesh obj)

         num-triangles (.getTriangleCount mesh)

         num-verticies (.getVertexCount mesh)

         uv-coord (partial uv-coord mesh)

         triangle-indices (partial triangle-indices mesh)

         receptors (touch-receptor-image obj)

         tris (triangles obj)

         ]

     (map

      (fn [[tri-1 tri-2 tri-3]]

        (let [width (.getWidth receptors)

              height (.getHeight receptors)

              uv-1 (uv-coord tri-1)

              uv-2 (uv-coord tri-2)

              uv-3 (uv-coord tri-3)

              x-coords (map #(.getX %) [uv-1 uv-2 uv-3])

              y-coords (map #(.getY %) [uv-1 uv-2 uv-3])

              max-x (Math/ceil (* width (apply max x-coords)))

              min-x (Math/floor (* width (apply min x-coords)))

              max-y (Math/ceil (* height (apply max y-coords)))

              min-y (Math/floor (* height (apply min y-coords)))

 

              image-1 (Vector2f. (* width (.getX uv-1))

                                 (* height (.getY uv-1)))

              image-2 (Vector2f. (* width (.getX uv-2))

                                 (* height (.getY uv-2)))

              image-3 (Vector2f. (* width (.getX uv-3))

                                 (* height (.getY uv-3)))

              left-corner

              (Vector2f. min-x min-y)

              ]

 

          (.setRoi receptors min-x min-y (- max-x min-x) (- max-y min-y))

          (let [processor  (.crop (.getProcessor receptors))]

            (map

             #(.add left-corner %)

             

             (filter 

              (partial

               inside-triangle?

               (.subtract image-1 left-corner)

               (.subtract image-2 left-corner)

               (.subtract image-3 left-corner))

              (white-coordinates processor))))

          )) (map triangle-indices (range num-triangles)))))

 

 #+end_src

 

 #+results: body-1

 : #'cortex.silly/test-joint

 

 

 * COMMENT purgatory

 #+begin_src clojure

 (defn bullet-trans []

   (let [obj-a (sphere 0.5 :color ColorRGBA/Red

                       :position (Vector3f. -10 5 0))

         obj-b (sphere 0.5 :color ColorRGBA/Blue

                       :position (Vector3f. -10 -5 0)

                       :mass 0)

         control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         swivel

         (.toRotationMatrix

          (doto (Quaternion.)

            (.fromAngleAxis (/ Math/PI 2)

                            Vector3f/UNIT_X)))]

     (doto 

         (ConeJoint.

          control-a control-b

          (Vector3f. 0 5 0)

          (Vector3f. 0 -5 0)

          swivel swivel)

       (.setLimit (* 0.6 (/ Math/PI 4))

                  (/ Math/PI 4)

                  (* Math/PI 0.8)))

     (world (nodify

             [obj-a obj-b])

             standard-debug-controls

             enable-debug

             no-op)))

 

 

 (defn bullet-trans* []

   (let [obj-a (box 1.5 0.5 0.5 :color ColorRGBA/Red

                    :position (Vector3f. 5 0 0)

                    :mass 90)

         obj-b (sphere 0.5 :color ColorRGBA/Blue

                       :position (Vector3f. -5 0 0)

                       :mass 0)

         control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         move-up? (atom nil)

         move-down? (atom nil)

         move-left? (atom nil)

         move-right? (atom nil)

         roll-left? (atom nil)

         roll-right? (atom nil)

         force 100

         swivel

         (.toRotationMatrix

          (doto (Quaternion.)

            (.fromAngleAxis (/ Math/PI 2)

                            Vector3f/UNIT_X)))

         x-move

         (doto (Matrix3f.)

           (.fromStartEndVectors Vector3f/UNIT_X

                                 (.normalize (Vector3f. 1 1 0))))

         

         timer (atom 0)]

     (doto 

         (ConeJoint.

          control-a control-b

          (Vector3f. -8 0 0)

          (Vector3f. 2 0 0)

          ;;swivel swivel

          ;;Matrix3f/IDENTITY Matrix3f/IDENTITY

          x-move Matrix3f/IDENTITY 

          )

       (.setCollisionBetweenLinkedBodys false)

       (.setLimit (* 1 (/ Math/PI 4))    ;; twist 

                  (* 1 (/ Math/PI 4))  ;; swing span in X-Y plane

                  (* 0 (/ Math/PI 4))))  ;; swing span in Y-Z plane

     (world (nodify

             [obj-a obj-b])

            (merge standard-debug-controls

                   {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))

                    "key-t" (fn [_ pressed?] (reset! move-down? pressed?))

                    "key-f" (fn [_ pressed?] (reset! move-left? pressed?))

                    "key-g" (fn [_ pressed?] (reset! move-right? pressed?))

                    "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))

                    "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     

            

            (fn [world]

              (enable-debug world)

              (set-gravity world Vector3f/ZERO)

              )

                            

            (fn [world _]

 

              (if @move-up?

                (.applyForce control-a

                               (Vector3f. force 0 0)

                               (Vector3f. 0 0 0)))

              (if @move-down?

                (.applyForce control-a

                              (Vector3f. (- force) 0 0)

                              (Vector3f. 0 0 0)))

              (if @move-left?

                (.applyForce control-a

                              (Vector3f. 0 force 0)

                              (Vector3f. 0 0 0)))

              (if @move-right?

                (.applyForce control-a

                              (Vector3f. 0 (- force) 0)

                              (Vector3f. 0 0 0)))

 

              (if @roll-left?

                (.applyForce control-a

                              (Vector3f. 0 0 force)

                              (Vector3f. 0 0 0)))

              (if @roll-right?

                (.applyForce control-a

                              (Vector3f. 0 0 (- force))

                              (Vector3f. 0 0 0)))

 

              (if (zero? (rem (swap! timer inc) 100))

                (.attachChild

                 (.getRootNode world)

                 (sphere 0.05 :color ColorRGBA/Yellow

                         :physical? false :position

                         (.getWorldTranslation obj-a)))))

            )

     ))

 

 (defn transform-trianglesdsd

   "Transform that converts each vertex in the first triangle

   into the corresponding vertex in the second triangle."

   [#^Triangle tri-1 #^Triangle tri-2]

   (let [in  [(.get1 tri-1)

              (.get2 tri-1)

              (.get3 tri-1)]

         out [(.get1 tri-2)

              (.get2 tri-2)

              (.get3 tri-2)]]

     (let [translate (doto (Matrix4f.) (.setTranslation (.negate (in 0))))

           in* [(.mult translate (in 0))

                (.mult translate (in 1))

                (.mult translate (in 2))]

           final-translation

           (doto (Matrix4f.)

             (.setTranslation (out 1)))

           

           rotate-1

           (doto (Matrix3f.)

             (.fromStartEndVectors

              (.normalize

               (.subtract

                (in* 1) (in* 0)))

              (.normalize

               (.subtract

                (out 1) (out 0)))))

           in** [(.mult rotate-1 (in* 0))

                 (.mult rotate-1 (in* 1))

                 (.mult rotate-1 (in* 2))]

           scale-factor-1

           (.mult

            (.normalize

             (.subtract

              (out 1)

              (out 0)))

            (/ (.length

                (.subtract (out 1)

                           (out 0)))

               (.length

                (.subtract (in** 1)

                           (in** 0)))))

           scale-1 (doto (Matrix4f.) (.setScale scale-factor-1))

           in*** [(.mult scale-1 (in** 0))

                  (.mult scale-1 (in** 1))

                  (.mult scale-1 (in** 2))]

           

           

           

           

           

           ]

       

       (dorun (map println in))

       (println)

       (dorun (map println in*))

       (println)

       (dorun (map println in**))

       (println)

       (dorun (map println in***))

       (println)

       

     ))))

 

 

 

 

 #+end_src

 

 

 * COMMENT generate source

 #+begin_src clojure :tangle ../src/cortex/silly.clj

 <<body-1>>

 #+end_src