cortex: org/test-creature.org comparison

comparison org/test-creature.org @ 83:14b604e955ed

still testing joints... Dylan is helping

author	Robert McIntyre <rlm@mit.edu>
date	Sat, 07 Jan 2012 00:58:47 -0700
parents	6b4ca076285e
children	4f5e2c629e45

comparison

equal deleted inserted replaced

-:6b4ca076285e
+:14b604e955ed
 (require 'cortex.import)
 (cortex.import/mega-import-jme3)
 (use '(cortex world util body hearing touch vision))
 (rlm.rlm-commands/help)
+(declare joint-create get-subjective-position)
+(defn load-bullet []
+(.start (world (Node.) {} no-op no-op)))
 (defn load-blender-model
 "Load a .blend file using an asset folder relative path."
 [^String model]
 (.loadModel
 (.toRotationMatrix
 (doto (Quaternion.)
 (.fromAngleAxis
 (float
 (.angleBetween
-pivot-a Vector3f/UNIT_X))
+(.normalize pivot-a) Vector3f/UNIT_X))
-(.cross (.normalize pivot-a)
+(.normalize
-Vector3f/UNIT_X))))
+(.cross pivot-a
-]
+Vector3f/UNIT_X)))))
+]
+(println-repl "pivot-a" pivot-a)
 (println-repl
-"angle between pivot-a (" pivot-a ") and UNIT_X is"
+"angle between pivot-a and UNIT_X is"
 (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))
+(println-repl "frame-a:" frame-a)
+(println-repl
+"frame-a moves Vector3f/UNIT_X to"
+(.mult frame-a Vector3f/UNIT_X ))
 (doto
 (ConeJoint.
 control-a
 control-b
 pivot-a
 pivot-b
-;; ;; frame-in-A
+;; frame-in-A
-frame-a
+;;frame-a
-frame-a
+;;frame-a
-;; (.toRotationMatrix
+(.toRotationMatrix
-;;  (doto (Quaternion.)
+(doto (Quaternion.)
-;;    (.fromAngles
+(.fromAngles
-;;     0 0 (* -1 (/ Math/PI 2)))))
+0 0 (* -0.5 (/ Math/PI 2)))))
-;; ;; frame-in-B
-;; (.toRotationMatrix
+;; frame-in-B
-;;  (doto (Quaternion.)
+(.toRotationMatrix
-;;    (.fromAngles
+(doto (Quaternion.)
-;;     0 0 (* -1.2 (/ Math/PI 2)))))
+(.fromAngles
+0 0 (* -0.5 (/ Math/PI 2)))))
 )
 (.setLimit (float limit-xz)
 (float limit-xy)
 (float twist))))))
 true
 (println-repl
 "joint-type" joint-type "not recognized")))
 (println-repl "could not find joint meta-data!"))))
 (defn assemble-creature [#^Node pieces joints]
 (dorun
 (map
 (fn [geom]
 world
 ))
 no-op)))
 (defn world-setup [joint]
-(let [top (doto
+(let [
+joint-position (Vector3f. 0 4 0)
+joint-rotation
+(.toRotationMatrix
+(.mult
+(doto (Quaternion.)
+(.fromAngleAxis
+(* 1 (/ Math/PI 4))
+(Vector3f. -1 0 0)))
+(doto (Quaternion.)
+(.fromAngleAxis
+(/ Math/PI 2)
+(Vector3f. 0 0 1)))))
+origin (doto
+(sphere 0.1 :physical? false :color ColorRGBA/Cyan
+:position (Vector3f. 0 0 0)))
+top (doto
 (sphere 0.1 :physical? false :color ColorRGBA/Yellow
-:position (Vector3f. 0 7 0))
+:position (.mult joint-rotation (Vector3f. 8 0 0)))
 (.addControl
 (RigidBodyControl.
-(CapsuleCollisionShape. 0.5 1.5 1) (float 15))))
+(CapsuleCollisionShape. 0.5 1.5 1) (float 20))))
 bottom (doto
 (sphere 0.1 :physical? false :color ColorRGBA/DarkGray
-:position (Vector3f. 0 -1 0))
+:position (Vector3f. 0 0 0))
 (.addControl
 (RigidBodyControl.
 (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))
-table (box 10 2 10 :position (Vector3f. 0 -6 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 :point)
-(doto
-(Point2PointJoint. a b
-(Vector3f. 0 -2 0)
-(Vector3f. 0  2 0))
-(.setCollisionBetweenLinkedBodys false))
-(= joint :hinge)
-(doto
-(HingeJoint.
-a b
-(Vector3f. 0 -2 0)
-(Vector3f. 0 2 0)
-(Vector3f. 0 0 1)
-(Vector3f. 0 0 1)
-)
-(.setCollisionBetweenLinkedBodys false)
-;;(.setLimit (- Math/PI) Math/PI)
-)
 (= joint :cone)
-;; note to self -- jbullet does NOT implement cone joints
-;; correctly. You must use plain ol' bullet for this to work.
-;; It's faster anyway, so whatever.
 (doto (ConeJoint.
 a b
-(Vector3f. 0 -5 0)
+(get-subjective-position joint-position top)
-(Vector3f. 0 2 0)
+(get-subjective-position joint-position bottom)
+joint-rotation
-(doto (Matrix3f.)
+joint-rotation
-(.fromStartEndVectors Vector3f/UNIT_X
+)
-Vector3f/UNIT_Y))
-(doto (Matrix3f.)
-(.fromStartEndVectors Vector3f/UNIT_X
+(.setLimit (* (/ 10) Math/PI)
-(.normalize
+(* (/ 4)  Math/PI)
-(Vector3f. 0 0 -1))))
+0)))
-)
+[origin top bottom table]))
-;;(.setAngularOnly true)
-(.setCollisionBetweenLinkedBodys false)
-(.setLimit (* 1 (/ Math/PI 4))
-(* 1 (/ Math/PI 2))
-(* 0 (/ Math/PI 4)))
-)
-(= joint :six)
-(doto
-(SixDofJoint.
-a b
-(Vector3f. 0 -2 0)
-(Vector3f. 0 2 0)
-;;(doto (Matrix3f.)
-;;    (.fromStartEndVectors Vector3f/UNIT_X
-;;                          Vector3f/UNIT_Y))
-;;(doto (Matrix3f.)
-;;  (.fromStartEndVectors Vector3f/UNIT_X
-;;                        Vector3f/UNIT_Y))
-true)
-(.setCollisionBetweenLinkedBodys false)
-(.setAngularLowerLimit (Vector3f. 0
-(- (/ Math/PI 2))
-0))
-(.setAngularUpperLimit (Vector3f. 0
-(/ Math/PI 2)
-0))
-(.setLinearLowerLimit  (Vector3f. 0 0 0))
-(.setLinearUpperLimit  (Vector3f. 0 0 0))
-)
-)
-[top bottom table]))
 (defn test-joint [joint]
-(let [[top bottom floor] (world-setup 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])
+(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?))
 (do
 ;;  (println-repl @timer)
 (.attachChild (.getRootNode world)
 (sphere 0.05 :color ColorRGBA/Yellow
 :position (.getWorldTranslation top)
-:physical? false))))
+: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?
 (Vector3f. -1 0 0))))
 (if @roll-right?
 (.applyTorque control
 (.mult (.getPhysicsRotation control)
 (Vector3f. 1 0 0))))))))
+;; please validate these.
+(defn get-subjective-position
+"Convert the world coordinates into coordinates relative to
+object (i.e. local coordinates), taking into account the rotation
+of object."
+[#^Vector3f world-coordinates object]
+;; I don't know if it's important to take into account the rotation
+;; of the object. If it isn't, then remove the multiplication-by-inverse.
+(.mult (.inverse (.getWorldRotation object))
+(.subtract world-coordinates (.getWorldTranslation object))))
+(defn get-subjective-rotation
+"cf get-subjective-position. Converts a rotation specified relative
+to the world's axes into a rotation specified in terms of the object's
+coordinate system."
+[#^Quaternion world-rotation object]
+(.mult (.inverse (.getWorldRotation object)) world-rotation))
+(defn joint-create "Connect objects 1 and 2 using a joint constraint. If
+only position is specified, creates a point-to-point joint at the
+given location
+in world coordinates. etc. etc. for other joints.
+To ensure consistency, I may alter this function
+so that it moves obj-1 to be at the apex of the cone.
+NOTE:
+In the usual construction method, you create a joint and, if your
+contraints are consistent, all the objects snap into position and
+orientation around it, otherwise the systen explodes.
+This construction method assumes that you have in mind a position and
+rotation for the joint, and that you have already put your objects
+at the required distances from that joint so that no snapping needs
+to occur. The radial distances ('pivot lengths') are therefore just set to be
+the pre-existing distances between the objects and the joint."
+[#^Node obj-1
+#^Node obj-2
+#^Vector3f joint-position
+#^Quaternion joint-rotation
+span-1
+span-2
+twist
+]
+(let
+[body-1 (.getControl obj-1 RigidBodyControl)
+body-2 (.getControl obj-2 RigidBodyControl)
+]
+(doto (ConeJoint.
+body-1
+body-2
+(get-subjective-position joint-position body-1)
+(get-subjective-position joint-position body-2)
+;; ALIGN X-AXIS OF OBJECT-1 WITH THE CENTRAL AXIS OF THE CONE TO
+;;LOWER THE RISK OF INCONSISTENCY
+;;(Matrix3f/IDENTITY)
+(.toRotationMatrix (get-subjective-rotation joint-rotation body-1))
+(.toRotationMatrix (get-subjective-rotation joint-rotation body-2))
+)
+(.setLimit
+span-1
+span-2
+twist))))
 #+end_src
 * COMMENT purgatory
 #+begin_src clojure

Mercurial > cortex

comparison org/test-creature.org @ 83:14b604e955ed