1.1 --- a/org/body.org	Fri Feb 03 06:41:16 2012 -0700
     1.2 +++ b/org/body.org	Fri Feb 03 06:47:05 2012 -0700
     1.3 @@ -9,7 +9,7 @@
     1.4  #+name: proprioception
     1.5  #+begin_src clojure 
     1.6  (ns cortex.body
     1.7 -  (:use (cortex world util))
     1.8 +  (:use (cortex world util sense))
     1.9    (:import
    1.10     com.jme3.math.Vector3f
    1.11     com.jme3.math.Quaternion
    1.12 @@ -20,6 +20,8 @@
    1.13     com.jme3.bounding.BoundingBox
    1.14     com.jme3.scene.Node))
    1.15  
    1.16 +(cortex.import/mega-import-jme3)
    1.17 +
    1.18  (defn jme-to-blender
    1.19    "Convert from JME coordinates to Blender coordinates"
    1.20    [#^Vector3f in]
    1.21 @@ -27,6 +29,14 @@
    1.22               (- (.getZ in))
    1.23               (.getY in)))
    1.24  
    1.25 +(defn blender-to-jme
    1.26 +  "Convert from Blender coordinates to JME coordinates"
    1.27 +  [#^Vector3f in]
    1.28 +  (Vector3f. (.getX in)
    1.29 +             (.getZ in)
    1.30 +             (- (.getY in))))
    1.31 +
    1.32 +
    1.33  (defn joint-targets
    1.34    "Return the two closest two objects to the joint object, ordered
    1.35    from bottom to top according to the joint's rotation."
    1.36 @@ -62,61 +72,160 @@
    1.37      (seq (.getChildren joint-node))
    1.38      (do (println-repl "could not find JOINTS node") [])))
    1.39  
    1.40 -(defn tap [obj direction force]
    1.41 -  (let [control (.getControl obj RigidBodyControl)]
    1.42 -    (.applyTorque
    1.43 -     control
    1.44 -     (.mult (.getPhysicsRotation control)
    1.45 -            (.mult (.normalize direction) (float force))))))
    1.46 +(defmulti joint-dispatch
    1.47 +  "Translate blender pseudo-joints into real JME joints."
    1.48 +  (fn [constraints & _] 
    1.49 +    (:type constraints)))
    1.50  
    1.51 +(defmethod joint-dispatch :point
    1.52 +  [constraints control-a control-b pivot-a pivot-b rotation]
    1.53 +  (println-repl "creating POINT2POINT joint")
    1.54 +  ;; bullet's point2point joints are BROKEN, so we must use the
    1.55 +  ;; generic 6DOF joint instead of an actual Point2Point joint!
    1.56  
    1.57 -(defn with-movement
    1.58 -  [object
    1.59 -   [up down left right roll-up roll-down :as keyboard]
    1.60 -   forces
    1.61 -   [root-node
    1.62 -    keymap
    1.63 -    intilization
    1.64 -    world-loop]]
    1.65 -  (let [add-keypress
    1.66 -        (fn [state keymap key]
    1.67 -          (merge keymap
    1.68 -                  {key
    1.69 -                   (fn [_ pressed?]
    1.70 -                     (reset! state pressed?))}))
    1.71 -        move-up? (atom false)
    1.72 -        move-down? (atom false)
    1.73 -        move-left? (atom false)
    1.74 -        move-right? (atom false)
    1.75 -        roll-left? (atom false)
    1.76 -        roll-right? (atom false)
    1.77 -        
    1.78 -        directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0)
    1.79 -                    (Vector3f. 0 0 1)(Vector3f. 0 0 -1)
    1.80 -                    (Vector3f. -1 0 0)(Vector3f. 1 0 0)]
    1.81 -        atoms [move-left? move-right? move-up? move-down? 
    1.82 -                 roll-left? roll-right?]
    1.83 +  ;; should be able to do this:
    1.84 +  (comment 
    1.85 +    (Point2PointJoint.
    1.86 +     control-a
    1.87 +     control-b
    1.88 +     pivot-a
    1.89 +     pivot-b))
    1.90  
    1.91 -        keymap* (reduce merge
    1.92 -                        (map #(add-keypress %1 keymap %2)
    1.93 -                             atoms
    1.94 -                             keyboard))
    1.95 -        
    1.96 -        splice-loop (fn []
    1.97 -                      (dorun
    1.98 -                       (map
    1.99 -                        (fn [sym direction force]
   1.100 -                          (if @sym
   1.101 -                            (tap object direction force)))
   1.102 -                        atoms directions forces)))
   1.103 +  ;; but instead we must do this:
   1.104 +  (println-repl "substuting 6DOF joint for POINT2POINT joint!")
   1.105 +  (doto
   1.106 +      (SixDofJoint.
   1.107 +       control-a
   1.108 +       control-b
   1.109 +       pivot-a
   1.110 +       pivot-b
   1.111 +       false)
   1.112 +    (.setLinearLowerLimit Vector3f/ZERO)
   1.113 +    (.setLinearUpperLimit Vector3f/ZERO)
   1.114 +    ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
   1.115 +    ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
   1.116  
   1.117 -        world-loop* (fn [world tpf]
   1.118 -                       (world-loop world tpf)
   1.119 -                       (splice-loop))]
   1.120 -    [root-node
   1.121 -     keymap*
   1.122 -     intilization
   1.123 -     world-loop*]))
   1.124 +))
   1.125 +
   1.126 +
   1.127 +(defmethod joint-dispatch :hinge
   1.128 +  [constraints control-a control-b pivot-a pivot-b rotation]
   1.129 +  (println-repl "creating HINGE joint")
   1.130 +  (let [axis
   1.131 +        (if-let
   1.132 +            [axis (:axis constraints)]
   1.133 +          axis
   1.134 +          Vector3f/UNIT_X)
   1.135 +        [limit-1 limit-2] (:limit constraints)
   1.136 +        hinge-axis
   1.137 +        (.mult
   1.138 +         rotation
   1.139 +         (blender-to-jme axis))]
   1.140 +    (doto
   1.141 +        (HingeJoint.
   1.142 +         control-a
   1.143 +         control-b
   1.144 +         pivot-a
   1.145 +         pivot-b
   1.146 +         hinge-axis
   1.147 +         hinge-axis)
   1.148 +      (.setLimit limit-1 limit-2))))
   1.149 +
   1.150 +(defmethod joint-dispatch :cone
   1.151 +  [constraints control-a control-b pivot-a pivot-b rotation]
   1.152 +  (let [limit-xz (:limit-xz constraints)
   1.153 +        limit-xy (:limit-xy constraints)
   1.154 +        twist    (:twist constraints)]
   1.155 +    
   1.156 +    (println-repl "creating CONE joint")
   1.157 +    (println-repl rotation)
   1.158 +    (println-repl
   1.159 +     "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
   1.160 +    (println-repl
   1.161 +     "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
   1.162 +    (println-repl
   1.163 +     "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
   1.164 +    (doto
   1.165 +        (ConeJoint.
   1.166 +         control-a
   1.167 +         control-b
   1.168 +         pivot-a
   1.169 +         pivot-b
   1.170 +         rotation
   1.171 +         rotation)
   1.172 +      (.setLimit (float limit-xz)
   1.173 +                 (float limit-xy)
   1.174 +                 (float twist)))))
   1.175 +
   1.176 +(defn connect
   1.177 +  "here are some examples:
   1.178 +   {:type :point}
   1.179 +   {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
   1.180 +   (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
   1.181 +
   1.182 +   {:type :cone :limit-xz 0]
   1.183 +                :limit-xy 0]
   1.184 +                :twist 0]}   (use XZY rotation mode in blender!)"
   1.185 +  [#^Node obj-a #^Node obj-b #^Node joint]
   1.186 +  (let [control-a (.getControl obj-a RigidBodyControl)
   1.187 +        control-b (.getControl obj-b RigidBodyControl)
   1.188 +        joint-center (.getWorldTranslation joint)
   1.189 +        joint-rotation (.toRotationMatrix (.getWorldRotation joint))
   1.190 +        pivot-a (world-to-local obj-a joint-center)
   1.191 +        pivot-b (world-to-local obj-b joint-center)]
   1.192 +   
   1.193 +    (if-let [constraints
   1.194 +             (map-vals
   1.195 +              eval
   1.196 +              (read-string
   1.197 +               (meta-data joint "joint")))]
   1.198 +      ;; A side-effect of creating a joint registers
   1.199 +      ;; it with both physics objects which in turn
   1.200 +      ;; will register the joint with the physics system
   1.201 +      ;; when the simulation is started.
   1.202 +      (do
   1.203 +        (println-repl "creating joint between"
   1.204 +                      (.getName obj-a) "and" (.getName obj-b))
   1.205 +        (joint-dispatch constraints
   1.206 +                        control-a control-b
   1.207 +                        pivot-a pivot-b
   1.208 +                        joint-rotation))
   1.209 +      (println-repl "could not find joint meta-data!"))))
   1.210 +
   1.211 +
   1.212 +
   1.213 +
   1.214 +(defn assemble-creature [#^Node pieces joints]
   1.215 +  (dorun
   1.216 +   (map
   1.217 +    (fn [geom]
   1.218 +      (let [physics-control
   1.219 +            (RigidBodyControl.
   1.220 +             (HullCollisionShape.
   1.221 +              (.getMesh geom))
   1.222 +             (if-let [mass (meta-data geom "mass")]
   1.223 +               (do
   1.224 +                 (println-repl
   1.225 +                  "setting" (.getName geom) "mass to" (float mass))
   1.226 +                 (float mass))
   1.227 +               (float 1)))]
   1.228 +               
   1.229 +        (.addControl geom physics-control)))
   1.230 +    (filter #(isa? (class %) Geometry )
   1.231 +            (node-seq pieces))))
   1.232 +  (dorun
   1.233 +   (map
   1.234 +    (fn [joint]
   1.235 +      (let [[obj-a obj-b] (joint-targets pieces joint)]
   1.236 +        (connect obj-a obj-b joint)))
   1.237 +    joints))
   1.238 +  pieces)
   1.239 +
   1.240 +
   1.241 +;; TODO implement a function that adds joints in the style of the
   1.242 +;; other sense functions
   1.243 +
   1.244 +
   1.245  
   1.246  (import java.awt.image.BufferedImage)
   1.247