# HG changeset patch # User Robert McIntyre # Date 1325923127 25200 # Node ID 14b604e955ed81f119141aaed191adec9993cfce # Parent 6b4ca076285eb932bb7dd4f2a1b00e4279d2cb41 still testing joints... Dylan is helping diff -r 6b4ca076285e -r 14b604e955ed assets/Models/creature1/one.blend Binary file assets/Models/creature1/one.blend has changed diff -r 6b4ca076285e -r 14b604e955ed org/cone-joints.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/org/cone-joints.txt Sat Jan 07 00:58:47 2012 -0700 @@ -0,0 +1,127 @@ + + + + +(ConeJoint + RigidBody A + RigidBody B + Vector3f PivotA + Vector3f PivotB + Matrix3f RotA + Matrix3f RotB +) + +Parameters: + +The specification of a cone joint depends on local coordinates, +that is coordinates relative to the given rigid bodies. + +*** +DIGRESSION ABOUT LOCAL COORDINATES + +Each rigid body has its own coordinate system. +If an object is placed, unrotated, at the origin of the world, +then its coordinate system is the same as the world's coordinate +system. In other words, the origin of the object coincides with the +origin of the world, and the XYZ axes of the object coincide with the +XYZ axes of the world. + +When you translate the object, its "origin" goes with it. +The origin of the object is always the center of the object. + +When you rotate the object, its axes go with it. +For example, if you rotate the object pi/2 radians righthandedly +about the Z axis, its own X axis will move to coincide with the +world's Y axis; its own Y axis will move to coincide with the +world's -X axis; its Z axis will remain aligned with the world's +Z-axis. + + +In such a way, you can see the way in which the translations and rotations of an +object alter its local coordinates. + + +## a haphazard elaboration of the above + +Converting from world coordinates to local coordinates amounts to +finding the transformation that undoes all the rotations and +translations that the Object has suffered, then applying that +undoing-transformation to the world coordinates. + + +position of obj +rot of obj +world axes ---------------> x -----------> obj axes + + +1. Start with the world coordinates of the Object and a test-object. +2. Subtract the world coordinates of the Object from each. Now the +Object is at the origin (though still has its rotation, if any) and +the test-object is somewhere else (irrelevant). +3. Rotate the whole world about the origin so that the Object becomes +unrotated. Now the object is unrotated at the origin, and the +test-object is somewhere else. +4. That somewhere-else is the coordinates of the test-object as seen +from the Object's own coordinate system (the system in which the Object is always +unrotated and centered at the origin) + +cf: get-subjective-rotation +cf: get-subjective-position + +*** + + +!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + +Now, to understand the ConeJoint constructor: +Suppose you have the position of the cone joint picked out, as well as its +orientation (orientation aka rotation matters because the limits of +rotation are taken with respect to the xy and xz plane of the _joint's_ +coordinate system. This is what the documentation means by "by +default, the x-axis"). + +Then: +Pivot-a is the position of the joint, as expressed in the coordinate +system of object-a. +Pivot-a is the position of the joint again, this time as expressed in +the coordinate system of object-b. + +(Use get-subjective-position to calculate this conveniently) + + +Rot-a is the orientation of the joint, as expressed in the +coordinate system of object-a. By default, it is aligned with a's +coordinate system, making the cone's axis along a's x-axis, and its +limits of rotation in a's xy and xz planes. + +Analogously for b. + +If inconsistent pivot locations are given, it's possible to recover by +moving the objects. I think the engine prefers to keep object b still +and move everything else, but it may not always be so. + +If inconsistent rotations are given ... I don't know what +happens. Beez happens. + + +Modulo a bunch of forced object migration because your pivots give +inconsistent indicators of where the joint should be expected, etc., this is +how the method works. + + + +# Another implementation option would have been to specify the positions and +# rotations of THREE things --- object A, object B, and the joint --- all in terms of world +# coordinates. This wouldn't be any more or less fragile (i.e. subject to explosions +# of inconsistency) but it might be nice to avoid those +# transformations into local coordinates, even if we had to introduce +# more parameters into such a new method +# (specifically, the position and orientation of the joint would be new). + +# I wouldn't mind because after all, don't we already need to keep in mind the position and orientation of the +# joint in the existing method? Lest it explode in inconsistency? I +# suggest we create a clojure method that creates cone joints in the +# transformation-free way. +# cf. joint-create, my first attempt + + + + diff -r 6b4ca076285e -r 14b604e955ed org/test-creature.org --- a/org/test-creature.org Fri Jan 06 02:03:43 2012 -0700 +++ b/org/test-creature.org Sat Jan 07 00:58:47 2012 -0700 @@ -50,6 +50,11 @@ (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] @@ -179,14 +184,19 @@ (.fromAngleAxis (float (.angleBetween - pivot-a Vector3f/UNIT_X)) - (.cross (.normalize pivot-a) - Vector3f/UNIT_X)))) - ] + (.normalize pivot-a) Vector3f/UNIT_X)) + (.normalize + (.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 @@ -197,22 +207,22 @@ pivot-b - ;; ;; frame-in-A - frame-a - frame-a + ;; frame-in-A + ;;frame-a + ;;frame-a - ;; (.toRotationMatrix - ;; (doto (Quaternion.) - ;; (.fromAngles - ;; 0 0 (* -1 (/ Math/PI 2))))) - + (.toRotationMatrix + (doto (Quaternion.) + (.fromAngles + 0 0 (* -0.5 (/ Math/PI 2))))) - ;; ;; frame-in-B - ;; (.toRotationMatrix - ;; (doto (Quaternion.) - ;; (.fromAngles - ;; 0 0 (* -1.2 (/ Math/PI 2))))) - + + ;; frame-in-B + (.toRotationMatrix + (doto (Quaternion.) + (.fromAngles + 0 0 (* -0.5 (/ Math/PI 2))))) + ) (.setLimit (float limit-xz) @@ -224,6 +234,7 @@ (println-repl "could not find joint meta-data!")))) + (defn assemble-creature [#^Node pieces joints] (dorun (map @@ -296,105 +307,63 @@ 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)) - (.addControl + :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) + (= joint :cone) + + (doto (ConeJoint. + a b + (get-subjective-position joint-position top) + (get-subjective-position joint-position bottom) + joint-rotation + joint-rotation + ) - ) - (.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) - (Vector3f. 0 2 0) - - (doto (Matrix3f.) - (.fromStartEndVectors Vector3f/UNIT_X - Vector3f/UNIT_Y)) - (doto (Matrix3f.) - (.fromStartEndVectors Vector3f/UNIT_X - (.normalize - (Vector3f. 0 0 -1)))) - ) - ;;(.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)) - - ) - + + (.setLimit (* (/ 10) Math/PI) + (* (/ 4) Math/PI) + 0))) + [origin top bottom table])) - - - - ) - - [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) @@ -405,7 +374,7 @@ 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?)) @@ -427,7 +396,12 @@ (.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) @@ -452,6 +426,93 @@ (.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