Mercurial > cortex

comparison org/test-creature.org @ 82:6b4ca076285e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
making some progress on cone joints...
author Robert McIntyre <rlm@mit.edu>
date Fri, 06 Jan 2012 02:03:43 -0700
parents 10f495560c59
children 14b604e955ed
comparison
equal deleted inserted replaced
81:10f495560c59 82:6b4ca076285e
171 (let [limit-xz (:limit-xz constraints) 171 (let [limit-xz (:limit-xz constraints)
172 limit-xy (:limit-xy constraints) 172 limit-xy (:limit-xy constraints)
173 twist (:twist constraints)] 173 twist (:twist constraints)]
174 174
175 (println-repl "creating CONE joint") 175 (println-repl "creating CONE joint")
176 (let [vector-1 176 (let [frame-a
177 (.mult (.getWorldRotation joint) 177 (.toRotationMatrix
178 Vector3f/UNIT_X) 178 (doto (Quaternion.)
179 vector-2 179 (.fromAngleAxis
180 (.normalize 180 (float
181 (.subtract 181 (.angleBetween
182 (.getWorldTranslation joint) 182 pivot-a Vector3f/UNIT_X))
183 (.getWorldTranslation obj-a))) 183 (.cross (.normalize pivot-a)
184 Vector3f/UNIT_X))))
184 ] 185 ]
185 (println-repl 186 (println-repl
186 "vector-1 :" vector-1) 187 "angle between pivot-a (" pivot-a ") and UNIT_X is"
187 (println-repl 188 (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))
188 "vector-2 :" vector-2)
189 189
190 190
191 191
192 (doto 192 (doto
193 (ConeJoint. 193 (ConeJoint.
194 control-a 194 control-a
195 control-b 195 control-b
196 pivot-a 196 pivot-a
197 pivot-b 197 pivot-b
198
199
200 ;; ;; frame-in-A
201 frame-a
202 frame-a
198 203
199 ;;(doto (Matrix3f.) 204 ;; (.toRotationMatrix
200 ;; (.fromStartEndVectors 205 ;; (doto (Quaternion.)
201 ;; Vector3f/UNIT_X 206 ;; (.fromAngles
202 ;; (.normalize 207 ;; 0 0 (* -1 (/ Math/PI 2)))))
203 ;; (.subtract
204 ;; (.getWorldTranslation joint)
205 ;; (.getWorldTranslation obj-a)))))
206
207
208 (.toRotationMatrix
209 (.getWorldRotation joint))
210 208
211 209
212 210 ;; ;; frame-in-B
213 (.toRotationMatrix 211 ;; (.toRotationMatrix
214 (.fromAngleAxis 212 ;; (doto (Quaternion.)
215 (Quaternion.) 213 ;; (.fromAngles
216 (.angleBetween Vector3f/UNIT_X pivot-a) 214 ;; 0 0 (* -1.2 (/ Math/PI 2)))))
217 (.cross Vector3f/UNIT_X pivot-a))) 215
218 216
219
220
221
222 ;; (doto (Matrix3f.)
223 ;; (.fromStartEndVectors
224 ;; Vector3f/UNIT_X
225 ;; (.normalize
226 ;; vector-1)))
227
228 ;; (doto (Matrix3f.)
229 ;; (.fromStartEndVectors
230 ;; Vector3f/UNIT_X
231 ;; vector-2))
232 ) 217 )
233 (.setLimit (float limit-xz) 218 (.setLimit (float limit-xz)
234 (float limit-xy) 219 (float limit-xy)
235 (float twist)))))) 220 (float twist))))))
236 true 221 true