|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 #ifndef _ODE_JOINT_H_ |
|
24 #define _ODE_JOINT_H_ |
|
25 |
|
26 |
|
27 #include "object.h" |
|
28 #include <ode/contact.h> |
|
29 #include "obstack.h" |
|
30 |
|
31 |
|
32 // joint flags |
|
33 enum { |
|
34 // if this flag is set, the joint was allocated in a joint group |
|
35 dJOINT_INGROUP = 1, |
|
36 |
|
37 // if this flag is set, the joint was attached with arguments (0,body). |
|
38 // our convention is to treat all attaches as (body,0), i.e. so node[0].body |
|
39 // is always nonzero, so this flag records the fact that the arguments were |
|
40 // swapped. |
|
41 dJOINT_REVERSE = 2, |
|
42 |
|
43 // if this flag is set, the joint can not have just one body attached to it, |
|
44 // it must have either zero or two bodies attached. |
|
45 dJOINT_TWOBODIES = 4 |
|
46 }; |
|
47 |
|
48 |
|
49 // there are two of these nodes in the joint, one for each connection to a |
|
50 // body. these are node of a linked list kept by each body of it's connecting |
|
51 // joints. but note that the body pointer in each node points to the body that |
|
52 // makes use of the *other* node, not this node. this trick makes it a bit |
|
53 // easier to traverse the body/joint graph. |
|
54 |
|
55 struct dxJointNode { |
|
56 dxJoint *joint; // pointer to enclosing dxJoint object |
|
57 dxBody *body; // *other* body this joint is connected to |
|
58 dxJointNode *next; // next node in body's list of connected joints |
|
59 }; |
|
60 |
|
61 |
|
62 struct dxJoint : public dObject { |
|
63 // naming convention: the "first" body this is connected to is node[0].body, |
|
64 // and the "second" body is node[1].body. if this joint is only connected |
|
65 // to one body then the second body is 0. |
|
66 |
|
67 // info returned by getInfo1 function. the constraint dimension is m (<=6). |
|
68 // i.e. that is the total number of rows in the jacobian. `nub' is the |
|
69 // number of unbounded variables (which have lo,hi = -/+ infinity). |
|
70 |
|
71 struct Info1 { |
|
72 int m,nub; |
|
73 }; |
|
74 |
|
75 // info returned by getInfo2 function |
|
76 |
|
77 struct Info2 { |
|
78 // integrator parameters: frames per second (1/stepsize), default error |
|
79 // reduction parameter (0..1). |
|
80 dReal fps,erp; |
|
81 |
|
82 // for the first and second body, pointers to two (linear and angular) |
|
83 // n*3 jacobian sub matrices, stored by rows. these matrices will have |
|
84 // been initialized to 0 on entry. if the second body is zero then the |
|
85 // J2xx pointers may be 0. |
|
86 dReal *J1l,*J1a,*J2l,*J2a; |
|
87 |
|
88 // elements to jump from one row to the next in J's |
|
89 int rowskip; |
|
90 |
|
91 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the |
|
92 // "constraint force mixing" vector. c is set to zero on entry, cfm is |
|
93 // set to a constant value (typically very small or zero) value on entry. |
|
94 dReal *c,*cfm; |
|
95 |
|
96 // lo and hi limits for variables (set to -/+ infinity on entry). |
|
97 dReal *lo,*hi; |
|
98 |
|
99 // findex vector for variables. see the LCP solver interface for a |
|
100 // description of what this does. this is set to -1 on entry. |
|
101 // note that the returned indexes are relative to the first index of |
|
102 // the constraint. |
|
103 int *findex; |
|
104 }; |
|
105 |
|
106 // virtual function table: size of the joint structure, function pointers. |
|
107 // we do it this way instead of using C++ virtual functions because |
|
108 // sometimes we need to allocate joints ourself within a memory pool. |
|
109 |
|
110 typedef void init_fn (dxJoint *joint); |
|
111 typedef void getInfo1_fn (dxJoint *joint, Info1 *info); |
|
112 typedef void getInfo2_fn (dxJoint *joint, Info2 *info); |
|
113 struct Vtable { |
|
114 int size; |
|
115 init_fn *init; |
|
116 getInfo1_fn *getInfo1; |
|
117 getInfo2_fn *getInfo2; |
|
118 int typenum; // a dJointTypeXXX type number |
|
119 }; |
|
120 |
|
121 Vtable *vtable; // virtual function table |
|
122 int flags; // dJOINT_xxx flags |
|
123 dxJointNode node[2]; // connections to bodies. node[1].body can be 0 |
|
124 dJointFeedback *feedback; // optional feedback structure |
|
125 dReal lambda[6]; // lambda generated by last step |
|
126 }; |
|
127 |
|
128 |
|
129 // joint group. NOTE: any joints in the group that have their world destroyed |
|
130 // will have their world pointer set to 0. |
|
131 |
|
132 struct dxJointGroup : public dBase { |
|
133 int num; // number of joints on the stack |
|
134 dObStack stack; // a stack of (possibly differently sized) dxJoint |
|
135 }; // objects. |
|
136 |
|
137 |
|
138 // common limit and motor information for a single joint axis of movement |
|
139 struct dxJointLimitMotor { |
|
140 dReal vel,fmax; // powered joint: velocity, max force |
|
141 dReal lostop,histop; // joint limits, relative to initial position |
|
142 dReal fudge_factor; // when powering away from joint limits |
|
143 dReal normal_cfm; // cfm to use when not at a stop |
|
144 dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit |
|
145 dReal bounce; // restitution factor |
|
146 // variables used between getInfo1() and getInfo2() |
|
147 int limit; // 0=free, 1=at lo limit, 2=at hi limit |
|
148 dReal limit_err; // if at limit, amount over limit |
|
149 |
|
150 void init (dxWorld *); |
|
151 void set (int num, dReal value); |
|
152 dReal get (int num); |
|
153 int testRotationalLimit (dReal angle); |
|
154 int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, |
|
155 const dVector3 ax1, int rotational); |
|
156 }; |
|
157 |
|
158 |
|
159 // ball and socket |
|
160 |
|
161 struct dxJointBall : public dxJoint { |
|
162 dVector3 anchor1; // anchor w.r.t first body |
|
163 dVector3 anchor2; // anchor w.r.t second body |
|
164 }; |
|
165 extern struct dxJoint::Vtable __dball_vtable; |
|
166 |
|
167 |
|
168 // hinge |
|
169 |
|
170 struct dxJointHinge : public dxJoint { |
|
171 dVector3 anchor1; // anchor w.r.t first body |
|
172 dVector3 anchor2; // anchor w.r.t second body |
|
173 dVector3 axis1; // axis w.r.t first body |
|
174 dVector3 axis2; // axis w.r.t second body |
|
175 dQuaternion qrel; // initial relative rotation body1 -> body2 |
|
176 dxJointLimitMotor limot; // limit and motor information |
|
177 }; |
|
178 extern struct dxJoint::Vtable __dhinge_vtable; |
|
179 |
|
180 |
|
181 // universal |
|
182 |
|
183 struct dxJointUniversal : public dxJoint { |
|
184 dVector3 anchor1; // anchor w.r.t first body |
|
185 dVector3 anchor2; // anchor w.r.t second body |
|
186 dVector3 axis1; // axis w.r.t first body |
|
187 dVector3 axis2; // axis w.r.t second body |
|
188 dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece |
|
189 dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 |
|
190 dxJointLimitMotor limot1; // limit and motor information for axis1 |
|
191 dxJointLimitMotor limot2; // limit and motor information for axis2 |
|
192 }; |
|
193 extern struct dxJoint::Vtable __duniversal_vtable; |
|
194 |
|
195 |
|
196 /** |
|
197 * The axisP must be perpendicular to axis2 |
|
198 * <PRE> |
|
199 * +-------------+ |
|
200 * | x | |
|
201 * +------------\+ |
|
202 * Prismatic articulation .. .. |
|
203 * | .. .. |
|
204 * \/ .. .. |
|
205 * +--------------+ --| __.. .. anchor2 |
|
206 * | x | .....|.......(__) .. |
|
207 * +--------------+ --| ^ < |
|
208 * |----------------------->| |
|
209 * Offset |--- Rotoide articulation |
|
210 * </PRE> |
|
211 */ |
|
212 struct dxJointPR : public dxJoint { |
|
213 |
|
214 dVector3 anchor2; ///< @brief Position of the rotoide articulation |
|
215 ///< w.r.t second body. |
|
216 ///< @note Position of body 2 in world frame + |
|
217 ///< anchor2 in world frame give the position |
|
218 ///< of the rotoide articulation |
|
219 dVector3 axisR1; ///< axis of the rotoide articulation w.r.t first body. |
|
220 ///< @note This is considered as axis1 from the parameter |
|
221 ///< view. |
|
222 dVector3 axisR2; ///< axis of the rotoide articulation w.r.t second body. |
|
223 ///< @note This is considered also as axis1 from the |
|
224 ///< parameter view |
|
225 dVector3 axisP1; ///< axis for the prismatic articulation w.r.t first body. |
|
226 ///< @note This is considered as axis2 in from the parameter |
|
227 ///< view |
|
228 dQuaternion qrel; ///< initial relative rotation body1 -> body2. |
|
229 dVector3 offset; ///< @brief vector between the body1 and the rotoide |
|
230 ///< articulation. |
|
231 ///< |
|
232 ///< Going from the first to the second in the frame |
|
233 ///< of body1. |
|
234 ///< That should be aligned with body1 center along axisP |
|
235 ///< This is calculated whe the axis are set. |
|
236 dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation. |
|
237 dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation. |
|
238 }; |
|
239 extern struct dxJoint::Vtable __dPR_vtable; |
|
240 |
|
241 |
|
242 |
|
243 // slider. if body2 is 0 then qrel is the absolute rotation of body1 and |
|
244 // offset is the position of body1 center along axis1. |
|
245 |
|
246 struct dxJointSlider : public dxJoint { |
|
247 dVector3 axis1; // axis w.r.t first body |
|
248 dQuaternion qrel; // initial relative rotation body1 -> body2 |
|
249 dVector3 offset; // point relative to body2 that should be |
|
250 // aligned with body1 center along axis1 |
|
251 dxJointLimitMotor limot; // limit and motor information |
|
252 }; |
|
253 extern struct dxJoint::Vtable __dslider_vtable; |
|
254 |
|
255 |
|
256 // contact |
|
257 |
|
258 struct dxJointContact : public dxJoint { |
|
259 int the_m; // number of rows computed by getInfo1 |
|
260 dContact contact; |
|
261 }; |
|
262 extern struct dxJoint::Vtable __dcontact_vtable; |
|
263 |
|
264 |
|
265 // hinge 2 |
|
266 |
|
267 struct dxJointHinge2 : public dxJoint { |
|
268 dVector3 anchor1; // anchor w.r.t first body |
|
269 dVector3 anchor2; // anchor w.r.t second body |
|
270 dVector3 axis1; // axis 1 w.r.t first body |
|
271 dVector3 axis2; // axis 2 w.r.t second body |
|
272 dReal c0,s0; // cos,sin of desired angle between axis 1,2 |
|
273 dVector3 v1,v2; // angle ref vectors embedded in first body |
|
274 dxJointLimitMotor limot1; // limit+motor info for axis 1 |
|
275 dxJointLimitMotor limot2; // limit+motor info for axis 2 |
|
276 dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) |
|
277 }; |
|
278 extern struct dxJoint::Vtable __dhinge2_vtable; |
|
279 |
|
280 |
|
281 // angular motor |
|
282 |
|
283 struct dxJointAMotor : public dxJoint { |
|
284 int num; // number of axes (0..3) |
|
285 int mode; // a dAMotorXXX constant |
|
286 int rel[3]; // what the axes are relative to (global,b1,b2) |
|
287 dVector3 axis[3]; // three axes |
|
288 dxJointLimitMotor limot[3]; // limit+motor info for axes |
|
289 dReal angle[3]; // user-supplied angles for axes |
|
290 // these vectors are used for calculating euler angles |
|
291 dVector3 reference1; // original axis[2], relative to body 1 |
|
292 dVector3 reference2; // original axis[0], relative to body 2 |
|
293 }; |
|
294 extern struct dxJoint::Vtable __damotor_vtable; |
|
295 |
|
296 |
|
297 struct dxJointLMotor : public dxJoint { |
|
298 int num; |
|
299 int rel[3]; |
|
300 dVector3 axis[3]; |
|
301 dxJointLimitMotor limot[3]; |
|
302 }; |
|
303 |
|
304 extern struct dxJoint::Vtable __dlmotor_vtable; |
|
305 |
|
306 |
|
307 // 2d joint, constrains to z == 0 |
|
308 |
|
309 struct dxJointPlane2D : public dxJoint |
|
310 { |
|
311 int row_motor_x; |
|
312 int row_motor_y; |
|
313 int row_motor_angle; |
|
314 dxJointLimitMotor motor_x; |
|
315 dxJointLimitMotor motor_y; |
|
316 dxJointLimitMotor motor_angle; |
|
317 }; |
|
318 |
|
319 extern struct dxJoint::Vtable __dplane2d_vtable; |
|
320 |
|
321 |
|
322 // fixed |
|
323 |
|
324 struct dxJointFixed : public dxJoint { |
|
325 dQuaternion qrel; // initial relative rotation body1 -> body2 |
|
326 dVector3 offset; // relative offset between the bodies |
|
327 }; |
|
328 extern struct dxJoint::Vtable __dfixed_vtable; |
|
329 |
|
330 |
|
331 // null joint, for testing only |
|
332 |
|
333 struct dxJointNull : public dxJoint { |
|
334 }; |
|
335 extern struct dxJoint::Vtable __dnull_vtable; |
|
336 |
|
337 |
|
338 #endif |