Mercurial > cortex

comparison org/body.org @ 160:33278bf028e7

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
refactored joints
author Robert McIntyre <rlm@mit.edu>
date Fri, 03 Feb 2012 06:47:05 -0700
parents 84c67be00abe
children 0b9ae09eaec3
comparison
equal deleted inserted replaced
159:75b6c2ebbf8e 160:33278bf028e7
7 7
8 * Proprioception 8 * Proprioception
9 #+name: proprioception 9 #+name: proprioception
10 #+begin_src clojure 10 #+begin_src clojure
11 (ns cortex.body 11 (ns cortex.body
12 (:use (cortex world util)) 12 (:use (cortex world util sense))
13 (:import 13 (:import
14 com.jme3.math.Vector3f 14 com.jme3.math.Vector3f
15 com.jme3.math.Quaternion 15 com.jme3.math.Quaternion
16 com.jme3.math.Vector2f 16 com.jme3.math.Vector2f
17 com.jme3.math.Matrix3f 17 com.jme3.math.Matrix3f
18 com.jme3.bullet.control.RigidBodyControl 18 com.jme3.bullet.control.RigidBodyControl
19 com.jme3.collision.CollisionResults 19 com.jme3.collision.CollisionResults
20 com.jme3.bounding.BoundingBox 20 com.jme3.bounding.BoundingBox
21 com.jme3.scene.Node)) 21 com.jme3.scene.Node))
22 22
23 (cortex.import/mega-import-jme3)
24
23 (defn jme-to-blender 25 (defn jme-to-blender
24 "Convert from JME coordinates to Blender coordinates" 26 "Convert from JME coordinates to Blender coordinates"
25 [#^Vector3f in] 27 [#^Vector3f in]
26 (Vector3f. (.getX in) 28 (Vector3f. (.getX in)
27 (- (.getZ in)) 29 (- (.getZ in))
28 (.getY in))) 30 (.getY in)))
31
32 (defn blender-to-jme
33 "Convert from Blender coordinates to JME coordinates"
34 [#^Vector3f in]
35 (Vector3f. (.getX in)
36 (.getZ in)
37 (- (.getY in))))
38
29 39
30 (defn joint-targets 40 (defn joint-targets
31 "Return the two closest two objects to the joint object, ordered 41 "Return the two closest two objects to the joint object, ordered
32 from bottom to top according to the joint's rotation." 42 from bottom to top according to the joint's rotation."
33 [#^Node parts #^Node joint] 43 [#^Node parts #^Node joint]
60 [#^Node creature] 70 [#^Node creature]
61 (if-let [joint-node (.getChild creature "joints")] 71 (if-let [joint-node (.getChild creature "joints")]
62 (seq (.getChildren joint-node)) 72 (seq (.getChildren joint-node))
63 (do (println-repl "could not find JOINTS node") []))) 73 (do (println-repl "could not find JOINTS node") [])))
64 74
65 (defn tap [obj direction force] 75 (defmulti joint-dispatch
66 (let [control (.getControl obj RigidBodyControl)] 76 "Translate blender pseudo-joints into real JME joints."
67 (.applyTorque 77 (fn [constraints & _]
68 control 78 (:type constraints)))
69 (.mult (.getPhysicsRotation control) 79
70 (.mult (.normalize direction) (float force)))))) 80 (defmethod joint-dispatch :point
71 81 [constraints control-a control-b pivot-a pivot-b rotation]
72 82 (println-repl "creating POINT2POINT joint")
73 (defn with-movement 83 ;; bullet's point2point joints are BROKEN, so we must use the
74 [object 84 ;; generic 6DOF joint instead of an actual Point2Point joint!
75 [up down left right roll-up roll-down :as keyboard] 85
76 forces 86 ;; should be able to do this:
77 [root-node 87 (comment
78 keymap 88 (Point2PointJoint.
79 intilization 89 control-a
80 world-loop]] 90 control-b
81 (let [add-keypress 91 pivot-a
82 (fn [state keymap key] 92 pivot-b))
83 (merge keymap 93
84 {key 94 ;; but instead we must do this:
85 (fn [_ pressed?] 95 (println-repl "substuting 6DOF joint for POINT2POINT joint!")
86 (reset! state pressed?))})) 96 (doto
87 move-up? (atom false) 97 (SixDofJoint.
88 move-down? (atom false) 98 control-a
89 move-left? (atom false) 99 control-b
90 move-right? (atom false) 100 pivot-a
91 roll-left? (atom false) 101 pivot-b
92 roll-right? (atom false) 102 false)
93 103 (.setLinearLowerLimit Vector3f/ZERO)
94 directions [(Vector3f. 0 1 0)(Vector3f. 0 -1 0) 104 (.setLinearUpperLimit Vector3f/ZERO)
95 (Vector3f. 0 0 1)(Vector3f. 0 0 -1) 105 ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
96 (Vector3f. -1 0 0)(Vector3f. 1 0 0)] 106 ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
97 atoms [move-left? move-right? move-up? move-down? 107
98 roll-left? roll-right?] 108 ))
99 109
100 keymap* (reduce merge 110
101 (map #(add-keypress %1 keymap %2) 111 (defmethod joint-dispatch :hinge
102 atoms 112 [constraints control-a control-b pivot-a pivot-b rotation]
103 keyboard)) 113 (println-repl "creating HINGE joint")
104 114 (let [axis
105 splice-loop (fn [] 115 (if-let
106 (dorun 116 [axis (:axis constraints)]
107 (map 117 axis
108 (fn [sym direction force] 118 Vector3f/UNIT_X)
109 (if @sym 119 [limit-1 limit-2] (:limit constraints)
110 (tap object direction force))) 120 hinge-axis
111 atoms directions forces))) 121 (.mult
112 122 rotation
113 world-loop* (fn [world tpf] 123 (blender-to-jme axis))]
114 (world-loop world tpf) 124 (doto
115 (splice-loop))] 125 (HingeJoint.
116 [root-node 126 control-a
117 keymap* 127 control-b
118 intilization 128 pivot-a
119 world-loop*])) 129 pivot-b
130 hinge-axis
131 hinge-axis)
132 (.setLimit limit-1 limit-2))))
133
134 (defmethod joint-dispatch :cone
135 [constraints control-a control-b pivot-a pivot-b rotation]
136 (let [limit-xz (:limit-xz constraints)
137 limit-xy (:limit-xy constraints)
138 twist (:twist constraints)]
139
140 (println-repl "creating CONE joint")
141 (println-repl rotation)
142 (println-repl
143 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
144 (println-repl
145 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
146 (println-repl
147 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
148 (doto
149 (ConeJoint.
150 control-a
151 control-b
152 pivot-a
153 pivot-b
154 rotation
155 rotation)
156 (.setLimit (float limit-xz)
157 (float limit-xy)
158 (float twist)))))
159
160 (defn connect
161 "here are some examples:
162 {:type :point}
163 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
164 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
165
166 {:type :cone :limit-xz 0]
167 :limit-xy 0]
168 :twist 0]} (use XZY rotation mode in blender!)"
169 [#^Node obj-a #^Node obj-b #^Node joint]
170 (let [control-a (.getControl obj-a RigidBodyControl)
171 control-b (.getControl obj-b RigidBodyControl)
172 joint-center (.getWorldTranslation joint)
173 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
174 pivot-a (world-to-local obj-a joint-center)
175 pivot-b (world-to-local obj-b joint-center)]
176
177 (if-let [constraints
178 (map-vals
179 eval
180 (read-string
181 (meta-data joint "joint")))]
182 ;; A side-effect of creating a joint registers
183 ;; it with both physics objects which in turn
184 ;; will register the joint with the physics system
185 ;; when the simulation is started.
186 (do
187 (println-repl "creating joint between"
188 (.getName obj-a) "and" (.getName obj-b))
189 (joint-dispatch constraints
190 control-a control-b
191 pivot-a pivot-b
192 joint-rotation))
193 (println-repl "could not find joint meta-data!"))))
194
195
196
197
198 (defn assemble-creature [#^Node pieces joints]
199 (dorun
200 (map
201 (fn [geom]
202 (let [physics-control
203 (RigidBodyControl.
204 (HullCollisionShape.
205 (.getMesh geom))
206 (if-let [mass (meta-data geom "mass")]
207 (do
208 (println-repl
209 "setting" (.getName geom) "mass to" (float mass))
210 (float mass))
211 (float 1)))]
212
213 (.addControl geom physics-control)))
214 (filter #(isa? (class %) Geometry )
215 (node-seq pieces))))
216 (dorun
217 (map
218 (fn [joint]
219 (let [[obj-a obj-b] (joint-targets pieces joint)]
220 (connect obj-a obj-b joint)))
221 joints))
222 pieces)
223
224
225 ;; TODO implement a function that adds joints in the style of the
226 ;; other sense functions
227
228
120 229
121 (import java.awt.image.BufferedImage) 230 (import java.awt.image.BufferedImage)
122 231
123 (defn draw-sprite [image sprite x y color ] 232 (defn draw-sprite [image sprite x y color ]
124 (dorun 233 (dorun