Mercurial > cortex

comparison org/body.org @ 206:df46a609fed9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
removed dead code
author Robert McIntyre <rlm@mit.edu>
date Thu, 09 Feb 2012 04:21:12 -0700
parents d3a2abfac405
children bb3b75bf1664
comparison
equal deleted inserted replaced
205:d3a2abfac405 206:df46a609fed9
566 Dylan -- I'll fill these in later 566 Dylan -- I'll fill these in later
567 - cortex.body 567 - cortex.body
568 - cortex.test.body 568 - cortex.test.body
569 - blender files 569 - blender files
570 570
571 * COMMENT Examples 571
572 572 * COMMENT Generate Source
573 #+name: test-body
574 #+begin_src clojure
575
576 (defn worm-segments
577 "Create multiple evenly spaced box segments. They're fabulous!"
578 [segment-length num-segments interstitial-space radius]
579 (letfn [(nth-segment
580 [n]
581 (box segment-length radius radius :mass 0.1
582 :position
583 (Vector3f.
584 (* 2 n (+ interstitial-space segment-length)) 0 0)
585 :name (str "worm-segment" n)
586 :color (ColorRGBA/randomColor)))]
587 (map nth-segment (range num-segments))))
588
589 (defn connect-at-midpoint
590 "Connect two physics objects with a Point2Point joint constraint at
591 the point equidistant from both objects' centers."
592 [segmentA segmentB]
593 (let [centerA (.getWorldTranslation segmentA)
594 centerB (.getWorldTranslation segmentB)
595 midpoint (.mult (.add centerA centerB) (float 0.5))
596 pivotA (.subtract midpoint centerA)
597 pivotB (.subtract midpoint centerB)
598
599 ;; A side-effect of creating a joint registers
600 ;; it with both physics objects which in turn
601 ;; will register the joint with the physics system
602 ;; when the simulation is started.
603 joint (Point2PointJoint.
604 (.getControl segmentA RigidBodyControl)
605 (.getControl segmentB RigidBodyControl)
606 pivotA
607 pivotB)]
608 segmentB))
609
610 (defn eve-worm
611 "Create a worm-like body bound by invisible joint constraints."
612 []
613 (let [segments (worm-segments 0.2 5 0.1 0.1)]
614 (dorun (map (partial apply connect-at-midpoint)
615 (partition 2 1 segments)))
616 (nodify "worm" segments)))
617
618 (defn worm-pattern
619 "This is a simple, mindless motor control pattern that drives the
620 second segment of the worm's body at an offset angle with
621 sinusoidally varying strength."
622 [time]
623 (let [angle (* Math/PI (/ 9 20))
624 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
625 [Vector3f/ZERO
626 (.mult
627 direction
628 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
629 Vector3f/ZERO
630 Vector3f/ZERO
631 Vector3f/ZERO]))
632
633 (defn test-motor-control
634 "Testing motor-control:
635 You should see a multi-segmented worm-like object fall onto the
636 table and begin writhing and moving."
637 []
638 (let [worm (eve-worm)
639 time (atom 0)
640 worm-motor-map (vector-motor-control worm)]
641 (world
642 (nodify [worm
643 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
644 :color ColorRGBA/Gray)])
645 standard-debug-controls
646 (fn [world]
647 (enable-debug world)
648 (light-up-everything world)
649 (comment
650 (com.aurellem.capture.Capture/captureVideo
651 world
652 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
653 )
654
655 (fn [_ _]
656 (swap! time inc)
657 (Thread/sleep 20)
658 (dorun (worm-motor-map
659 (worm-pattern @time)))))))
660
661
662
663 (defn join-at-point [obj-a obj-b world-pivot]
664 (cortex.silly/joint-dispatch
665 {:type :point}
666 (.getControl obj-a RigidBodyControl)
667 (.getControl obj-b RigidBodyControl)
668 (cortex.silly/world-to-local obj-a world-pivot)
669 (cortex.silly/world-to-local obj-b world-pivot)
670 nil
671 ))
672
673 (import com.jme3.bullet.collision.PhysicsCollisionObject)
674
675 (defn blab-* []
676 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
677 :mass 0 :color ColorRGBA/Green)
678 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
679 :mass 1 :color ColorRGBA/Red)
680 connection-point (Vector3f. 1.2 0 0)
681 root (nodify [hand finger])]
682
683 (join-at-point hand finger (Vector3f. 1.2 0 0))
684
685 (.setCollisionGroup
686 (.getControl hand RigidBodyControl)
687 PhysicsCollisionObject/COLLISION_GROUP_NONE)
688 (world
689 root
690 standard-debug-controls
691 (fn [world]
692 (enable-debug world)
693 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
694 (set-gravity world Vector3f/ZERO)
695 )
696 no-op)))
697 (comment
698
699 (defn proprioception-debug-window
700 []
701 (let [time (atom 0)]
702 (fn [prop-data]
703 (if (= 0 (rem (swap! time inc) 40))
704 (println-repl prop-data)))))
705 )
706
707 (comment
708 (dorun
709 (map
710 (comp
711 println-repl
712 (fn [[p y r]]
713 (format
714 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
715 p y r)))
716 prop-data)))
717
718
719
720
721 (defn test-proprioception
722 "Testing proprioception:
723 You should see two foating bars, and a printout of pitch, yaw, and
724 roll. Pressing key-r/key-t should move the blue bar up and down and
725 change only the value of pitch. key-f/key-g moves it side to side
726 and changes yaw. key-v/key-b will spin the blue segment clockwise
727 and counterclockwise, and only affect roll."
728 []
729 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
730 :mass 0 :color ColorRGBA/Green :name "hand")
731 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
732 :mass 1 :color ColorRGBA/Red :name "finger")
733 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
734 :position (Vector3f. 0 1.2 0)
735 :rotation (doto (Quaternion.)
736 (.fromAngleAxis
737 (/ Math/PI 2)
738 (Vector3f. 0 0 1)))
739 :physical? false)
740 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
741 creature (nodify [hand finger joint-node])
742 finger-control (.getControl finger RigidBodyControl)
743 hand-control (.getControl hand RigidBodyControl)]
744
745
746 (let
747 ;; *******************************************
748
749 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
750 :mass 0 :color ColorRGBA/Gray)
751
752 root (nodify [creature floor])
753 prop (joint-proprioception creature joint-node)
754 prop-view (proprioception-debug-window)
755
756 controls
757 (merge standard-debug-controls
758 {"key-o"
759 (fn [_ _] (.setEnabled finger-control true))
760 "key-p"
761 (fn [_ _] (.setEnabled finger-control false))
762 "key-k"
763 (fn [_ _] (.setEnabled hand-control true))
764 "key-l"
765 (fn [_ _] (.setEnabled hand-control false))
766 "key-i"
767 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
768 "key-period"
769 (fn [world _]
770 (.setEnabled finger-control false)
771 (.setEnabled hand-control false)
772 (.rotate creature (doto (Quaternion.)
773 (.fromAngleAxis
774 (float (/ Math/PI 15))
775 (Vector3f. 0 0 -1))))
776
777 (.setEnabled finger-control true)
778 (.setEnabled hand-control true)
779 (set-gravity world (Vector3f. 0 0 0))
780 )
781
782
783 }
784 )
785
786 ]
787 (comment
788 (.setCollisionGroup
789 (.getControl hand RigidBodyControl)
790 PhysicsCollisionObject/COLLISION_GROUP_NONE)
791 )
792 (apply
793 world
794 (with-movement
795 hand
796 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
797 [10 10 10 10 1 1]
798 (with-movement
799 finger
800 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
801 [1 1 10 10 10 10]
802 [root
803 controls
804 (fn [world]
805 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
806 (set-gravity world (Vector3f. 0 0 0))
807 (light-up-everything world))
808 (fn [_ _] (prop-view (list (prop))))]))))))
809
810 #+end_src
811
812 #+results: test-body
813 : #'cortex.test.body/test-proprioception
814
815
816 * COMMENT code-limbo
817 #+begin_src clojure
818 ;;(.loadModel
819 ;; (doto (asset-manager)
820 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
821 ;; "Models/person/person.blend")
822
823
824 (defn load-blender-model
825 "Load a .blend file using an asset folder relative path."
826 [^String model]
827 (.loadModel
828 (doto (asset-manager)
829 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
830 model))
831
832
833 (defn view-model [^String model]
834 (view
835 (.loadModel
836 (doto (asset-manager)
837 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
838 model)))
839
840 (defn load-blender-scene [^String model]
841 (.loadModel
842 (doto (asset-manager)
843 (.registerLoader BlenderLoader (into-array String ["blend"])))
844 model))
845
846 (defn worm
847 []
848 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
849
850 (defn oto
851 []
852 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
853
854 (defn sinbad
855 []
856 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
857
858 (defn worm-blender
859 []
860 (first (seq (.getChildren (load-blender-model
861 "Models/anim2/simple-worm.blend")))))
862
863 (defn body
864 "given a node with a SkeletonControl, will produce a body sutiable
865 for AI control with movement and proprioception."
866 [node]
867 (let [skeleton-control (.getControl node SkeletonControl)
868 krc (KinematicRagdollControl.)]
869 (comment
870 (dorun
871 (map #(.addBoneName krc %)
872 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
873 ;;"mid2" "mid3" "tail" "head"]
874 )))
875 (.addControl node krc)
876 (.setRagdollMode krc)
877 )
878 node
879 )
880 (defn show-skeleton [node]
881 (let [sd
882
883 (doto
884 (SkeletonDebugger. "aurellem-skel-debug"
885 (skel node))
886 (.setMaterial (green-x-ray)))]
887 (.attachChild node sd)
888 node))
889
890
891
892 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
893
894 ;; this could be a good way to give objects special properties like
895 ;; being eyes and the like
896
897 (.getUserData
898 (.getChild
899 (load-blender-model "Models/property/test.blend") 0)
900 "properties")
901
902 ;; the properties are saved along with the blender file.
903 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
904
905
906
907
908 (defn init-debug-skel-node
909 [f debug-node skeleton]
910 (let [bones
911 (map #(.getBone skeleton %)
912 (range (.getBoneCount skeleton)))]
913 (dorun (map #(.setUserControl % true) bones))
914 (dorun (map (fn [b]
915 (println (.getName b)
916 " -- " (f b)))
917 bones))
918 (dorun
919 (map #(.attachChild
920 debug-node
921 (doto
922 (sphere 0.1
923 :position (f %)
924 :physical? false)
925 (.setMaterial (green-x-ray))))
926 bones)))
927 debug-node)
928
929 (import jme3test.bullet.PhysicsTestHelper)
930
931
932 (defn test-zzz [the-worm world value]
933 (if (not value)
934 (let [skeleton (skel the-worm)]
935 (println-repl "enabling bones")
936 (dorun
937 (map
938 #(.setUserControl (.getBone skeleton %) true)
939 (range (.getBoneCount skeleton))))
940
941
942 (let [b (.getBone skeleton 2)]
943 (println-repl "moving " (.getName b))
944 (println-repl (.getLocalPosition b))
945 (.setUserTransforms b
946 Vector3f/UNIT_X
947 Quaternion/IDENTITY
948 ;;(doto (Quaternion.)
949 ;; (.fromAngles (/ Math/PI 2)
950 ;; 0
951 ;; 0
952
953 (Vector3f. 1 1 1))
954 )
955
956 (println-repl "hi! <3"))))
957
958
959 (defn test-ragdoll []
960
961 (let [the-worm
962
963 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
964 (doto (show-skeleton (worm-blender))
965 (.setLocalTranslation (Vector3f. 0 10 0))
966 ;;(worm)
967 ;;(oto)
968 ;;(sinbad)
969 )
970 ]
971
972
973 (.start
974 (world
975 (doto (Node.)
976 (.attachChild the-worm))
977 {"key-return" (fire-cannon-ball)
978 "key-space" (partial test-zzz the-worm)
979 }
980 (fn [world]
981 (light-up-everything world)
982 (PhysicsTestHelper/createPhysicsTestWorld
983 (.getRootNode world)
984 (asset-manager)
985 (.getPhysicsSpace
986 (.getState (.getStateManager world) BulletAppState)))
987 (set-gravity world Vector3f/ZERO)
988 ;;(.setTimer world (NanoTimer.))
989 ;;(org.lwjgl.input.Mouse/setGrabbed false)
990 )
991 no-op
992 )
993
994
995 )))
996
997
998 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
999 ;;; here is the ragdoll stuff
1000
1001 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
1002 (def mesh worm-mesh)
1003
1004 (.getFloatBuffer mesh VertexBuffer$Type/Position)
1005 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
1006 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
1007
1008
1009 (defn position [index]
1010 (.get
1011 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
1012 index))
1013
1014 (defn bones [index]
1015 (.get
1016 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
1017 index))
1018
1019 (defn bone-weights [index]
1020 (.get
1021 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
1022 index))
1023
1024
1025
1026 (defn vertex-bones [vertex]
1027 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
1028
1029 (defn vertex-weights [vertex]
1030 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
1031
1032 (defn vertex-position [index]
1033 (let [offset (* index 3)]
1034 (Vector3f. (position offset)
1035 (position (inc offset))
1036 (position (inc(inc offset))))))
1037
1038 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
1039
1040 (defn bone-control-color [index]
1041 (get {[1 0 0 0] ColorRGBA/Red
1042 [1 2 0 0] ColorRGBA/Magenta
1043 [2 0 0 0] ColorRGBA/Blue}
1044 (vertex-bones index)
1045 ColorRGBA/White))
1046
1047 (defn influence-color [index bone-num]
1048 (get
1049 {(float 0) ColorRGBA/Blue
1050 (float 0.5) ColorRGBA/Green
1051 (float 1) ColorRGBA/Red}
1052 ;; find the weight of the desired bone
1053 ((zipmap (vertex-bones index)(vertex-weights index))
1054 bone-num)
1055 ColorRGBA/Blue))
1056
1057 (def worm-vertices (set (map vertex-info (range 60))))
1058
1059
1060 (defn test-info []
1061 (let [points (Node.)]
1062 (dorun
1063 (map #(.attachChild points %)
1064 (map #(sphere 0.01
1065 :position (vertex-position %)
1066 :color (influence-color % 1)
1067 :physical? false)
1068 (range 60))))
1069 (view points)))
1070
1071
1072 (defrecord JointControl [joint physics-space]
1073 PhysicsControl
1074 (setPhysicsSpace [this space]
1075 (dosync
1076 (ref-set (:physics-space this) space))
1077 (.addJoint space (:joint this)))
1078 (update [this tpf])
1079 (setSpatial [this spatial])
1080 (render [this rm vp])
1081 (getPhysicsSpace [this] (deref (:physics-space this)))
1082 (isEnabled [this] true)
1083 (setEnabled [this state]))
1084
1085 (defn add-joint
1086 "Add a joint to a particular object. When the object is added to the
1087 PhysicsSpace of a simulation, the joint will also be added"
1088 [object joint]
1089 (let [control (JointControl. joint (ref nil))]
1090 (.addControl object control))
1091 object)
1092
1093
1094 (defn hinge-world
1095 []
1096 (let [sphere1 (sphere)
1097 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
1098 joint (Point2PointJoint.
1099 (.getControl sphere1 RigidBodyControl)
1100 (.getControl sphere2 RigidBodyControl)
1101 Vector3f/ZERO (Vector3f. 3 3 3))]
1102 (add-joint sphere1 joint)
1103 (doto (Node. "hinge-world")
1104 (.attachChild sphere1)
1105 (.attachChild sphere2))))
1106
1107
1108 (defn test-joint []
1109 (view (hinge-world)))
1110
1111 ;; (defn copier-gen []
1112 ;; (let [count (atom 0)]
1113 ;; (fn [in]
1114 ;; (swap! count inc)
1115 ;; (clojure.contrib.duck-streams/copy
1116 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
1117 ;; ;;/home/r/tmp/mao-test/clojure-images
1118 ;; (format "%08d.png" @count)))))))
1119 ;; (defn decrease-framerate []
1120 ;; (map
1121 ;; (copier-gen)
1122 ;; (sort
1123 ;; (map first
1124 ;; (partition
1125 ;; 4
1126 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
1127 ;; (file-seq
1128 ;; (file-str
1129 ;; "/home/r/media/anime/mao-temp/images"))))))))
1130
1131
1132
1133 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
1134
1135 (defn proprioception
1136 "Create a proprioception map that reports the rotations of the
1137 various limbs of the creature's body"
1138 [creature]
1139 [#^Node creature]
1140 (let [
1141 nodes (node-seq creature)
1142 joints
1143 (map
1144 :joint
1145 (filter
1146 #(isa? (class %) JointControl)
1147 (reduce
1148 concat
1149 (map (fn [node]
1150 (map (fn [num] (.getControl node num))
1151 (range (.getNumControls node))))
1152 nodes))))]
1153 (fn []
1154 (reduce concat (map relative-positions (list (first joints)))))))
1155
1156
1157 (defn skel [node]
1158 (doto
1159 (.getSkeleton
1160 (.getControl node SkeletonControl))
1161 ;; this is necessary to force the skeleton to have accurate world
1162 ;; transforms before it is rendered to the screen.
1163 (.resetAndUpdate)))
1164
1165 (defn green-x-ray []
1166 (doto (Material. (asset-manager)
1167 "Common/MatDefs/Misc/Unshaded.j3md")
1168 (.setColor "Color" ColorRGBA/Green)
1169 (-> (.getAdditionalRenderState)
1170 (.setDepthTest false))))
1171
1172 (defn test-worm []
1173 (.start
1174 (world
1175 (doto (Node.)
1176 ;;(.attachChild (point-worm))
1177 (.attachChild (load-blender-model
1178 "Models/anim2/joint-worm.blend"))
1179
1180 (.attachChild (box 10 1 10
1181 :position (Vector3f. 0 -2 0) :mass 0
1182 :color (ColorRGBA/Gray))))
1183 {
1184 "key-space" (fire-cannon-ball)
1185 }
1186 (fn [world]
1187 (enable-debug world)
1188 (light-up-everything world)
1189 ;;(.setTimer world (NanoTimer.))
1190 )
1191 no-op)))
1192
1193
1194
1195 ;; defunct movement stuff
1196 (defn torque-controls [control]
1197 (let [torques
1198 (concat
1199 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
1200 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
1201 [Vector3f/UNIT_X])]
1202 (map (fn [torque-axis]
1203 (fn [torque]
1204 (.applyTorque
1205 control
1206 (.mult (.mult (.getPhysicsRotation control)
1207 torque-axis)
1208 (float
1209 (* (.getMass control) torque))))))
1210 torques)))
1211
1212 (defn motor-map
1213 "Take a creature and generate a function that will enable fine
1214 grained control over all the creature's limbs."
1215 [#^Node creature]
1216 (let [controls (keep #(.getControl % RigidBodyControl)
1217 (node-seq creature))
1218 limb-controls (reduce concat (map torque-controls controls))
1219 body-control (partial map #(%1 %2) limb-controls)]
1220 body-control))
1221
1222 (defn test-motor-map
1223 "see how torque works."
1224 []
1225 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
1226 :mass 1 :color ColorRGBA/Green)
1227 motor-map (motor-map finger)]
1228 (world
1229 (nodify [finger
1230 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
1231 :color ColorRGBA/Gray)])
1232 standard-debug-controls
1233 (fn [world]
1234 (set-gravity world Vector3f/ZERO)
1235 (light-up-everything world)
1236 (.setTimer world (NanoTimer.)))
1237 (fn [_ _]
1238 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
1239 0]))))))
1240
1241 (defn joint-proprioception [#^Node parts #^Node joint]
1242 (let [[obj-a obj-b] (joint-targets parts joint)
1243 joint-rot (.getWorldRotation joint)
1244 pre-inv-a (.inverse (.getWorldRotation obj-a))
1245 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
1246 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
1247 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
1248
1249 x Vector3f/UNIT_Y
1250 y Vector3f/UNIT_Z
1251 z Vector3f/UNIT_X
1252
1253
1254 tmp-rot-a (.getWorldRotation obj-a)]
1255 (println-repl "x:" (.mult tmp-rot-a x))
1256 (println-repl "y:" (.mult tmp-rot-a y))
1257 (println-repl "z:" (.mult tmp-rot-a z))
1258 (println-repl "rot-a" (.getWorldRotation obj-a))
1259 (println-repl "rot-b" (.getWorldRotation obj-b))
1260 (println-repl "joint-rot" joint-rot)
1261 ;; this function will report proprioceptive information for the
1262 ;; joint.
1263 (fn []
1264 ;; x is the "twist" axis, y and z are the "bend" axes
1265 (let [rot-a (.getWorldRotation obj-a)
1266 ;;inv-a (.inverse rot-a)
1267 rot-b (.getWorldRotation obj-b)
1268 ;;relative (.mult rot-b inv-a)
1269 basis (doto (Matrix3f.)
1270 (.setColumn 0 (.mult rot-a x))
1271 (.setColumn 1 (.mult rot-a y))
1272 (.setColumn 2 (.mult rot-a z)))
1273 rotation-about-joint
1274 (doto (Quaternion.)
1275 (.fromRotationMatrix
1276 (.mult (.invert basis)
1277 (.toRotationMatrix rot-b))))
1278 [yaw roll pitch]
1279 (seq (.toAngles rotation-about-joint nil))]
1280 ;;return euler angles of the quaternion around the new basis
1281 [yaw roll pitch]))))
1282
1283 #+end_src
1284
1285
1286
1287
1288
1289
1290
1291 * COMMENT generate Source
1292 #+begin_src clojure :tangle ../src/cortex/body.clj 573 #+begin_src clojure :tangle ../src/cortex/body.clj
1293 <<body-header>> 574 <<body-header>>
1294 <<body-1>> 575 <<body-1>>
1295 <<joints-2>> 576 <<joints-2>>
1296 <<joints-3>> 577 <<joints-3>>
1307 <<test-4>> 588 <<test-4>>
1308 #+end_src 589 #+end_src
1309 590
1310 591
1311 592
593