[cells-cvs] CVS cells-ode

[phildebrandt]

Update of /project/cells/cvsroot/cells-ode
In directory clnet:/tmp/cvs-serv1675

Modified Files:
	joints.lisp 
Log Message:
fixed joints


--- /project/cells/cvsroot/cells-ode/joints.lisp	2008/02/08 18:09:31	1.1
+++ /project/cells/cvsroot/cells-ode/joints.lisp	2008/02/08 18:19:45	1.2
@@ -49,7 +49,7 @@
    (angle-rate :type number :read-only t))
   (:default-initargs :ode-id (call-ode joint-create-hinge ((*world* object) ((null-pointer))))))
 
-#+broken
+#+slider-fixed
 (def-ode-model (slider-joint :ode-class joint :ode-joint slider :joint-axes 2) (joint)
   ((axis :type vector :result-arg t :auto-update nil)
    (position :type number :read-only t)
@@ -78,26 +78,27 @@
    (anglerate2 :type number :read-only t))
   (:default-initargs :ode-id (call-ode joint-create-hinge2 ((*world* object) ((null-pointer))))))
 
-#+broken
-(defmodel a-motor-axis ()
-  ((axis :initarg :axis :accessor axis :initform (c-in #(1 0 0)))
-   (angle :initarg :angle :accessor angle :initform (c-in 0))
-   (relative-to :initarg :relative-to :accessor relative-to :initform (c-in :body1))
-   #+future-ode (rate :initarg :rate :accessor :rate :initform (c-in 0))
-   (num :initarg :num :reader num)
-   (owner :initarg :owner :initform (error "need to supply :owner for a-motor-axis") :reader owner)))
-
-(def-ode-model (a-motor-joint :ode-class joint :ode-joint a-motor :joint-axes 2) (joint)
-  ((mode :type int :auto-update nil :initform (c-in ode:+a-motor-user+)) ; ode:+a-motor-user+ or ode:+a-motor-euler+
-   (num-axes :type int :auto-update nil :initform (c-in 0))
-   (axes :ode nil :initform (c? (coerce
-				 (let (res)
-				  (dotimes (i (^num-axes) res)
-				    (push (make-instance 'a-motor-axis
-							 :owner self
-							 :num i) res))
-				  (nreverse res)) 'vector)))) ; a vector of num-axes a-motor-axis models
-  (:default-initargs :ode-id (call-ode joint-create-a-motor ((*world* object) ((null-pointer))))))
+#+a-motor-fixed
+(progn
+ (defmodel a-motor-axis ()
+   ((axis :initarg :axis :accessor axis :initform (c-in #(1 0 0)))
+    (angle :initarg :angle :accessor angle :initform (c-in 0))
+    (relative-to :initarg :relative-to :accessor relative-to :initform (c-in :body1))
+    #+future-ode (rate :initarg :rate :accessor :rate :initform (c-in 0))
+    (num :initarg :num :reader num)
+    (owner :initarg :owner :initform (error "need to supply :owner for a-motor-axis") :reader owner)))
+ 
+ (def-ode-model (a-motor-joint :ode-class joint :ode-joint a-motor :joint-axes 2) (joint)
+   ((mode :type int :auto-update nil :initform (c-in ode:+a-motor-user+)) ; ode:+a-motor-user+ or ode:+a-motor-euler+
+    (num-axes :type int :auto-update nil :initform (c-in 0))
+    (axes :ode nil :initform (c? (coerce
+				  (let (res)
+				    (dotimes (i (^num-axes) res)
+				      (push (make-instance 'a-motor-axis
+							   :owner self
+							   :num i) res))
+				    (nreverse res)) 'vector)))) ; a vector of num-axes a-motor-axis models
+   (:default-initargs :ode-id (call-ode joint-create-a-motor ((*world* object) ((null-pointer)))))))
 
 ;;;
 ;;; contact joint
@@ -122,55 +123,54 @@
 (defmethod bodies ((self joint))
   (list (get-body self 0) (get-body self 1)))
 
-;;; 
-;;; TODO set/get feedback
-;;; 
 
 (def-ode-fun are-connected ((body1 object) (body2 object)) bool)
 (def-ode-fun are-connected-excluding ((body1 object) (body2 object) (joint-type int)) bool)
 
 ;;; AMotor stuff
 
-(define-constant +a-motor-axis-rel+ '(:global :body1 :body2))
-
-(def-ode-method set-a-motor-axis ((self a-motor-joint joint) (axis-num int) (relative-to int) (axis vector))
-  nil
-  (let ((relative-to (or (cl:position relative-to +a-motor-axis-rel+)
-			 (error "axis-X-rel has to be one of :global, :body1, :body2 (and not ~a)" relative-to))))
-    (call-ode-method)))
-
-(def-ode-method get-a-motor-axis ((self a-motor-joint joint) (axis-num int) (result vector)))
-(def-ode-method get-a-motor-axis-rel ((self a-motor-joint joint) (axis-num int))
-  int
-  (nth (call-ode-method) +a-motor-axis-rel+))
+#+a-motor-fixed
+(progn
+ (define-constant +a-motor-axis-rel+ '(:global :body1 :body2))
+
+ (def-ode-method set-a-motor-axis ((self a-motor-joint joint) (axis-num int) (relative-to int) (axis vector))
+   nil
+   (let ((relative-to (or (cl:position relative-to +a-motor-axis-rel+)
+			  (error "axis-X-rel has to be one of :global, :body1, :body2 (and not ~a)" relative-to))))
+     (call-ode-method)))
+
+ (def-ode-method get-a-motor-axis ((self a-motor-joint joint) (axis-num int) (result vector)))
+ (def-ode-method get-a-motor-axis-rel ((self a-motor-joint joint) (axis-num int))
+   int
+   (nth (call-ode-method) +a-motor-axis-rel+))
 
-(def-ode-method set-a-motor-angle ((self a-motor-joint joint) (axis-num int) (angle number)))
-(def-ode-method get-a-motor-angle ((self a-motor-joint joint) (axis-num int)) number)
+ (def-ode-method set-a-motor-angle ((self a-motor-joint joint) (axis-num int) (angle number)))
+ (def-ode-method get-a-motor-angle ((self a-motor-joint joint) (axis-num int)) number)
 
-#+future-ode (def-ode-method get-a-motor-angle-rate ((self a-motor-joint joint) (axis-num int)) number)
+ #+future-ode (def-ode-method get-a-motor-angle-rate ((self a-motor-joint joint) (axis-num int)) number)
 ;;; PH 02.2008 -- this is not supported in ODE 0.8
 
 ;;; AMotor cellified
 
-(defobserver axis ((self a-motor-axis) newval)
-  (when newval
-    (set-a-motor-axis (owner self) (num self) (relative-to self) newval)))
-
-(defobserver relative-to ((self a-motor-axis) newval)
-  (when newval
-    (set-a-motor-axis (owner self) (num self) newval (axis self))))
-
-(defobserver angle ((self a-motor-axis) newval)
-  (when newval
-    (set-a-motor-angle (owner self) (num self)  newval)))
-
-(defmethod update :after ((self a-motor-joint))
-  (loop for num from 0 below (num-axes self)
-     do (with-accessors ((axis axis) (angle angle) #+future-ode (rate rate))
-	    (aref (axes self) num)
-	  (setf axis (get-a-motor-axis self num)
-		angle (get-a-motor-angle self num))
-	  #+future-ode (setf rate (get-a-motor-angle-rate self num)))))
+ (defobserver axis ((self a-motor-axis) newval)
+   (when newval
+     (set-a-motor-axis (owner self) (num self) (relative-to self) newval)))
+
+ (defobserver relative-to ((self a-motor-axis) newval)
+   (when newval
+     (set-a-motor-axis (owner self) (num self) newval (axis self))))
+
+ (defobserver angle ((self a-motor-axis) newval)
+   (when newval
+     (set-a-motor-angle (owner self) (num self)  newval)))
+
+ (defmethod update :after ((self a-motor-joint))
+   (loop for num from 0 below (num-axes self)
+      do (with-accessors ((axis axis) (angle angle) #+future-ode (rate rate))
+	     (aref (axes self) num)
+	   (setf axis (get-a-motor-axis self num)
+		 angle (get-a-motor-angle self num))
+	   #+future-ode (setf rate (get-a-motor-angle-rate self num))))))
 
 ;;; TODO:  Add Torque directly