/[cells]/cells-ode/joints.lisp
ViewVC logotype

Contents of /cells-ode/joints.lisp

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1.5 - (show annotations)
Mon Jun 2 14:12:53 2008 UTC (5 years, 10 months ago) by phildebrandt
Branch: MAIN
CVS Tags: HEAD
Changes since 1.4: +11 -0 lines
attach joints by using slots body-1, body-2
1 #|
2
3 Cells-ODE -- A cells driven interface to cl-ode
4
5 Copyright (C) 2008 by Peter Hildebrandt
6
7 This library is free software; you can redistribute it and/or
8 modify it under the terms of the Lisp Lesser GNU Public License
9 (http://opensource.franz.com/preamble.html), known as the LLGPL.
10
11 This library is distributed WITHOUT ANY WARRANTY; without even
12 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13
14 See the Lisp Lesser GNU Public License for more details.
15
16 |#
17
18
19 (in-package :c-ode)
20
21 ;;;
22 ;;; joint
23 ;;;
24
25 (def-ode-model joint ()
26 ((joint-type :type int :ode-slot type :read-only t) ; returns one constant +ode:joint-type-...+
27 (feedback-struct :ode nil :cell nil :initform (foreign-alloc 'ode:joint-feedback))
28 (body-1 :ode nil)
29 (body-2 :ode nil)
30 (force-1 :ode nil)
31 (torque-1 :ode nil)
32 (force-2 :ode nil)
33 (torque-2 :ode nil))
34 (:default-initargs :ode-id (error "You must instantiate a subtype of joint!")))
35
36 (defmethod initialize-instance :after ((self joint) &rest initargs)
37 (call-ode joint-set-feedback ((self object) ((feedback-struct self)))))
38
39 (defmethod ode-destroy ((self joint))
40 (call-ode joint-destroy ((self object)))
41 (foreign-free (feedback-struct self))
42 (call-next-method))
43
44 (defmacro propagate-feedback (feedback-struct joint)
45 `(with-foreign-slots ((ode:f-1 ode:t-1 ode:f-2 ode:t-2) ,feedback-struct ode:joint-feedback)
46 ,@(loop for (ode slot) on '(f-1 force-1 t-1 torque-1 f-2 force-2 t-2 torque-2) by #'cddr
47 collect `(setf (,slot ,joint) (coerce (loop for i from 0 below 3 collecting (mem-aref ,(intern (string ode) :ode) 'real i)) 'vector)))))
48
49 (defmethod update :after ((self joint))
50 (unless (typep self 'contact-joint)
51 (propagate-feedback (feedback-struct self) self)))
52
53
54 ;;;
55 ;;; joint types
56 ;;;
57
58 (def-ode-model (ball-joint :ode-class joint :ode-joint ball :joint-axes 0) (joint)
59 ((anchor :type vector :result-arg t :auto-update nil)
60 (anchor2 :type vector :result-arg t :read-only t))
61 (:default-initargs :ode-id (call-ode joint-create-ball ((*world* object) ((null-pointer))))))
62
63
64 (def-ode-model (hinge-joint :ode-class joint :ode-joint hinge :joint-axes 1) (joint)
65 ((anchor :type vector :result-arg t :auto-update nil)
66 (axis :type vector :result-arg t :auto-update nil)
67 (anchor2 :type vector :result-arg t :read-only t)
68 (angle :type number :read-only t)
69 (angle-rate :type number :read-only t))
70 (:default-initargs :ode-id (call-ode joint-create-hinge ((*world* object) ((null-pointer))))))
71
72 #+slider-fixed
73 (def-ode-model (slider-joint :ode-class joint :ode-joint slider :joint-axes 2) (joint)
74 ((axis :type vector :result-arg t :auto-update nil)
75 (position :type number :read-only t)
76 (positionrate :type number :read-only t))
77 (:default-initargs :ode-id (call-ode joint-create-slider ((*world* object) ((null-pointer))))))
78
79 (def-ode-model (universal-joint :ode-class joint :ode-joint universal :joint-axes 2) (joint)
80 ((anchor :type vector :result-arg t :auto-update nil)
81 (axis1 :type vector :result-arg t :auto-update nil)
82 (axis2 :type vector :result-arg t :auto-update nil)
83 (anchor2 :type vector :result-arg t :read-only t)
84 (angle1 :type number :read-only t)
85 (angle1rate :type number :read-only t)
86 (angle2 :type number :read-only t)
87 (angle2rate :type number :read-only t))
88 (:default-initargs :ode-id (call-ode joint-create-universal ((*world* object) ((null-pointer))))))
89
90 (def-ode-model (hinge-2-joint :ode-class joint :ode-joint hinge2 :joint-axes 2) (joint)
91 ((anchor :type vector :result-arg t :auto-update nil)
92 (axis1 :type vector :result-arg t :auto-update nil)
93 (axis2 :type vector :result-arg t :auto-update nil)
94 (anchor2 :type vector :result-arg t :read-only t)
95 (angle1 :type number :read-only t)
96 (anglerate1 :type number :read-only t)
97 (angle2 :type number :read-only t)
98 (anglerate2 :type number :read-only t))
99 (:default-initargs :ode-id (call-ode joint-create-hinge2 ((*world* object) ((null-pointer))))))
100
101 #+a-motor-fixed
102 (progn
103 (defmodel a-motor-axis ()
104 ((axis :initarg :axis :accessor axis :initform (c-in #(1 0 0)))
105 (angle :initarg :angle :accessor angle :initform (c-in 0))
106 (relative-to :initarg :relative-to :accessor relative-to :initform (c-in :body1))
107 #+future-ode (rate :initarg :rate :accessor :rate :initform (c-in 0))
108 (num :initarg :num :reader num)
109 (owner :initarg :owner :initform (error "need to supply :owner for a-motor-axis") :reader owner)))
110
111 (def-ode-model (a-motor-joint :ode-class joint :ode-joint a-motor :joint-axes 2) (joint)
112 ((mode :type int :auto-update nil :initform (c-in ode:+a-motor-user+)) ; ode:+a-motor-user+ or ode:+a-motor-euler+
113 (num-axes :type int :auto-update nil :initform (c-in 0))
114 (axes :ode nil :initform (c? (coerce
115 (let (res)
116 (dotimes (i (^num-axes) res)
117 (push (make-instance 'a-motor-axis
118 :owner self
119 :num i) res))
120 (nreverse res)) 'vector)))) ; a vector of num-axes a-motor-axis models
121 (:default-initargs :ode-id (call-ode joint-create-a-motor ((*world* object) ((null-pointer)))))))
122
123 ;;;
124 ;;; contact joint
125 ;;;
126
127 (def-ode-model (contact-joint :ode-class joint :ode-joint contact) (joint)
128 ()
129 (:default-initargs :ode-id (error "Use mk-contact-joint to create a contact joint")))
130
131 (defun mk-contact-joint (joint-group contact &rest initargs)
132 (let ((joint (apply #'make-instance 'contact-joint :ode-id (call-ode joint-create-contact ((*world* object) (joint-group object) contact)) initargs)))
133 (push joint (joints joint-group))
134 joint))
135
136 ;;;
137 ;;; Attaching
138 ;;;
139
140 (def-ode-method attach ((self joint) (body1 object) (body2 object)))
141 (def-ode-method set-fixed ((self joint)))
142 (def-ode-method get-body ((self joint) (index int)) object)
143
144 (defobserver body-1 ((self joint))
145 (when (and new-value (^body-2))
146 (attach self new-value (^body-2))))
147
148 (defobserver body-2 ((self joint))
149 (when (and new-value (^body-1))
150 (attach self (^body-1) new-value)))
151
152 (defmethod bodies ((self joint))
153 (list (get-body self 0) (get-body self 1)))
154
155
156 (def-ode-fun are-connected ((body1 object) (body2 object)) bool)
157 (def-ode-fun are-connected-excluding ((body1 object) (body2 object) (joint-type int)) bool)
158
159 ;;; AMotor stuff
160
161 #+a-motor-fixed
162 (progn
163 (define-constant +a-motor-axis-rel+ '(:global :body1 :body2))
164
165 (def-ode-method set-a-motor-axis ((self a-motor-joint joint) (axis-num int) (relative-to int) (axis vector))
166 nil
167 (let ((relative-to (or (cl:position relative-to +a-motor-axis-rel+)
168 (error "axis-X-rel has to be one of :global, :body1, :body2 (and not ~a)" relative-to))))
169 (call-ode-method)))
170
171 (def-ode-method get-a-motor-axis ((self a-motor-joint joint) (axis-num int) (result vector)))
172 (def-ode-method get-a-motor-axis-rel ((self a-motor-joint joint) (axis-num int))
173 int
174 (nth (call-ode-method) +a-motor-axis-rel+))
175
176 (def-ode-method set-a-motor-angle ((self a-motor-joint joint) (axis-num int) (angle number)))
177 (def-ode-method get-a-motor-angle ((self a-motor-joint joint) (axis-num int)) number)
178
179 #+future-ode (def-ode-method get-a-motor-angle-rate ((self a-motor-joint joint) (axis-num int)) number)
180 ;;; PH 02.2008 -- this is not supported in ODE 0.8
181
182 ;;; AMotor cellified
183
184 (defobserver axis ((self a-motor-axis) newval)
185 (when newval
186 (set-a-motor-axis (owner self) (num self) (relative-to self) newval)))
187
188 (defobserver relative-to ((self a-motor-axis) newval)
189 (when newval
190 (set-a-motor-axis (owner self) (num self) newval (axis self))))
191
192 (defobserver angle ((self a-motor-axis) newval)
193 (when newval
194 (set-a-motor-angle (owner self) (num self) newval)))
195
196 (defmethod update :after ((self a-motor-joint))
197 (loop for num from 0 below (num-axes self)
198 do (with-accessors ((axis axis) (angle angle) #+future-ode (rate rate))
199 (aref (axes self) num)
200 (setf axis (get-a-motor-axis self num)
201 angle (get-a-motor-angle self num))
202 #+future-ode (setf rate (get-a-motor-angle-rate self num))))))
203
204 ;;; TODO: Add Torque directly
205
206 ;;;
207 ;;; joint-group
208 ;;;
209
210 (def-ode-model joint-group ()
211 ((joints :ode nil :initform (c-in nil)))
212 (:default-initargs
213 :ode-id (error "use mk-joint-group to create a joint-group")))
214
215 (defun mk-joint-group (max-size &rest initargs)
216 (apply #'make-instance 'joint-group :ode-id (call-ode joint-group-create ((max-size int))) initargs))
217
218 (defmethod ode-destroy ((self joint-group))
219 (empty self)
220 (call-ode joint-group-destroy ((self object)))
221 (call-next-method))
222
223 (def-ode-method empty ((self joint-group))
224 nil
225 (dolist (joint (joints self)) (ode-destroy joint ))
226 (call-ode-method))

  ViewVC Help
Powered by ViewVC 1.1.5