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 "objects.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 | /******************** breakable joint contribution ***********************/ |
---|
62 | struct dxJointBreakInfo : public dBase { |
---|
63 | int flags; |
---|
64 | dReal b1MaxF[3]; // maximum force on body 1 |
---|
65 | dReal b1MaxT[3]; // maximum torque on body 1 |
---|
66 | dReal b2MaxF[3]; // maximum force on body 2 |
---|
67 | dReal b2MaxT[3]; // maximum torque on body 2 |
---|
68 | dJointBreakCallback *callback; // function that is called when this joint breaks |
---|
69 | }; |
---|
70 | /*************************************************************************/ |
---|
71 | |
---|
72 | struct dxJoint : public dObject { |
---|
73 | // naming convention: the "first" body this is connected to is node[0].body, |
---|
74 | // and the "second" body is node[1].body. if this joint is only connected |
---|
75 | // to one body then the second body is 0. |
---|
76 | |
---|
77 | // info returned by getInfo1 function. the constraint dimension is m (<=6). |
---|
78 | // i.e. that is the total number of rows in the jacobian. `nub' is the |
---|
79 | // number of unbounded variables (which have lo,hi = -/+ infinity). |
---|
80 | |
---|
81 | struct Info1 { |
---|
82 | int m,nub; |
---|
83 | }; |
---|
84 | |
---|
85 | // info returned by getInfo2 function |
---|
86 | |
---|
87 | struct Info2 { |
---|
88 | // integrator parameters: frames per second (1/stepsize), default error |
---|
89 | // reduction parameter (0..1). |
---|
90 | dReal fps,erp; |
---|
91 | |
---|
92 | // for the first and second body, pointers to two (linear and angular) |
---|
93 | // n*3 jacobian sub matrices, stored by rows. these matrices will have |
---|
94 | // been initialized to 0 on entry. if the second body is zero then the |
---|
95 | // J2xx pointers may be 0. |
---|
96 | dReal *J1l,*J1a,*J2l,*J2a; |
---|
97 | |
---|
98 | // elements to jump from one row to the next in J's |
---|
99 | int rowskip; |
---|
100 | |
---|
101 | // right hand sides of the equation J*v = c + cfm * lambda. cfm is the |
---|
102 | // "constraint force mixing" vector. c is set to zero on entry, cfm is |
---|
103 | // set to a constant value (typically very small or zero) value on entry. |
---|
104 | dReal *c,*cfm; |
---|
105 | |
---|
106 | // lo and hi limits for variables (set to -/+ infinity on entry). |
---|
107 | dReal *lo,*hi; |
---|
108 | |
---|
109 | // findex vector for variables. see the LCP solver interface for a |
---|
110 | // description of what this does. this is set to -1 on entry. |
---|
111 | // note that the returned indexes are relative to the first index of |
---|
112 | // the constraint. |
---|
113 | int *findex; |
---|
114 | }; |
---|
115 | |
---|
116 | // virtual function table: size of the joint structure, function pointers. |
---|
117 | // we do it this way instead of using C++ virtual functions because |
---|
118 | // sometimes we need to allocate joints ourself within a memory pool. |
---|
119 | |
---|
120 | typedef void init_fn (dxJoint *joint); |
---|
121 | typedef void getInfo1_fn (dxJoint *joint, Info1 *info); |
---|
122 | typedef void getInfo2_fn (dxJoint *joint, Info2 *info); |
---|
123 | struct Vtable { |
---|
124 | int size; |
---|
125 | init_fn *init; |
---|
126 | getInfo1_fn *getInfo1; |
---|
127 | getInfo2_fn *getInfo2; |
---|
128 | int typenum; // a dJointTypeXXX type number |
---|
129 | }; |
---|
130 | |
---|
131 | Vtable *vtable; // virtual function table |
---|
132 | int flags; // dJOINT_xxx flags |
---|
133 | dxJointNode node[2]; // connections to bodies. node[1].body can be 0 |
---|
134 | dJointFeedback *feedback; // optional feedback structure |
---|
135 | |
---|
136 | /******************** breakable joint contribution ***********************/ |
---|
137 | // optional break info structure. if this is not NULL the the joint is |
---|
138 | // breakable. |
---|
139 | dxJointBreakInfo *breakInfo; |
---|
140 | /*************************************************************************/ |
---|
141 | }; |
---|
142 | |
---|
143 | |
---|
144 | // joint group. NOTE: any joints in the group that have their world destroyed |
---|
145 | // will have their world pointer set to 0. |
---|
146 | |
---|
147 | struct dxJointGroup : public dBase { |
---|
148 | int num; // number of joints on the stack |
---|
149 | dObStack stack; // a stack of (possibly differently sized) dxJoint |
---|
150 | }; // objects. |
---|
151 | |
---|
152 | |
---|
153 | // common limit and motor information for a single joint axis of movement |
---|
154 | struct dxJointLimitMotor { |
---|
155 | dReal vel,fmax; // powered joint: velocity, max force |
---|
156 | dReal lostop,histop; // joint limits, relative to initial position |
---|
157 | dReal fudge_factor; // when powering away from joint limits |
---|
158 | dReal normal_cfm; // cfm to use when not at a stop |
---|
159 | dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit |
---|
160 | dReal bounce; // restitution factor |
---|
161 | // variables used between getInfo1() and getInfo2() |
---|
162 | int limit; // 0=free, 1=at lo limit, 2=at hi limit |
---|
163 | dReal limit_err; // if at limit, amount over limit |
---|
164 | |
---|
165 | void init (dxWorld *); |
---|
166 | void set (int num, dReal value); |
---|
167 | dReal get (int num); |
---|
168 | int testRotationalLimit (dReal angle); |
---|
169 | int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, |
---|
170 | dVector3 ax1, int rotational); |
---|
171 | }; |
---|
172 | |
---|
173 | |
---|
174 | // ball and socket |
---|
175 | |
---|
176 | struct dxJointBall : public dxJoint { |
---|
177 | dVector3 anchor1; // anchor w.r.t first body |
---|
178 | dVector3 anchor2; // anchor w.r.t second body |
---|
179 | }; |
---|
180 | extern struct dxJoint::Vtable __dball_vtable; |
---|
181 | |
---|
182 | |
---|
183 | // hinge |
---|
184 | |
---|
185 | struct dxJointHinge : public dxJoint { |
---|
186 | dVector3 anchor1; // anchor w.r.t first body |
---|
187 | dVector3 anchor2; // anchor w.r.t second body |
---|
188 | dVector3 axis1; // axis w.r.t first body |
---|
189 | dVector3 axis2; // axis w.r.t second body |
---|
190 | dQuaternion qrel; // initial relative rotation body1 -> body2 |
---|
191 | dxJointLimitMotor limot; // limit and motor information |
---|
192 | }; |
---|
193 | extern struct dxJoint::Vtable __dhinge_vtable; |
---|
194 | |
---|
195 | |
---|
196 | // universal |
---|
197 | |
---|
198 | struct dxJointUniversal : public dxJoint { |
---|
199 | dVector3 anchor1; // anchor w.r.t first body |
---|
200 | dVector3 anchor2; // anchor w.r.t second body |
---|
201 | dVector3 axis1; // axis w.r.t first body |
---|
202 | dVector3 axis2; // axis w.r.t second body |
---|
203 | dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece |
---|
204 | dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 |
---|
205 | dxJointLimitMotor limot1; // limit and motor information for axis1 |
---|
206 | dxJointLimitMotor limot2; // limit and motor information for axis2 |
---|
207 | }; |
---|
208 | extern struct dxJoint::Vtable __duniversal_vtable; |
---|
209 | |
---|
210 | |
---|
211 | // slider. if body2 is 0 then qrel is the absolute rotation of body1 and |
---|
212 | // offset is the position of body1 center along axis1. |
---|
213 | |
---|
214 | struct dxJointSlider : public dxJoint { |
---|
215 | dVector3 axis1; // axis w.r.t first body |
---|
216 | dQuaternion qrel; // initial relative rotation body1 -> body2 |
---|
217 | dVector3 offset; // point relative to body2 that should be |
---|
218 | // aligned with body1 center along axis1 |
---|
219 | dxJointLimitMotor limot; // limit and motor information |
---|
220 | }; |
---|
221 | extern struct dxJoint::Vtable __dslider_vtable; |
---|
222 | |
---|
223 | |
---|
224 | // contact |
---|
225 | |
---|
226 | struct dxJointContact : public dxJoint { |
---|
227 | int the_m; // number of rows computed by getInfo1 |
---|
228 | dContact contact; |
---|
229 | }; |
---|
230 | extern struct dxJoint::Vtable __dcontact_vtable; |
---|
231 | |
---|
232 | |
---|
233 | // hinge 2 |
---|
234 | |
---|
235 | struct dxJointHinge2 : public dxJoint { |
---|
236 | dVector3 anchor1; // anchor w.r.t first body |
---|
237 | dVector3 anchor2; // anchor w.r.t second body |
---|
238 | dVector3 axis1; // axis 1 w.r.t first body |
---|
239 | dVector3 axis2; // axis 2 w.r.t second body |
---|
240 | dReal c0,s0; // cos,sin of desired angle between axis 1,2 |
---|
241 | dVector3 v1,v2; // angle ref vectors embedded in first body |
---|
242 | dxJointLimitMotor limot1; // limit+motor info for axis 1 |
---|
243 | dxJointLimitMotor limot2; // limit+motor info for axis 2 |
---|
244 | dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) |
---|
245 | }; |
---|
246 | extern struct dxJoint::Vtable __dhinge2_vtable; |
---|
247 | |
---|
248 | |
---|
249 | // angular motor |
---|
250 | |
---|
251 | struct dxJointAMotor : public dxJoint { |
---|
252 | int num; // number of axes (0..3) |
---|
253 | int mode; // a dAMotorXXX constant |
---|
254 | int rel[3]; // what the axes are relative to (global,b1,b2) |
---|
255 | dVector3 axis[3]; // three axes |
---|
256 | dxJointLimitMotor limot[3]; // limit+motor info for axes |
---|
257 | dReal angle[3]; // user-supplied angles for axes |
---|
258 | // these vectors are used for calculating euler angles |
---|
259 | dVector3 reference1; // original axis[2], relative to body 1 |
---|
260 | dVector3 reference2; // original axis[0], relative to body 2 |
---|
261 | }; |
---|
262 | extern struct dxJoint::Vtable __damotor_vtable; |
---|
263 | |
---|
264 | |
---|
265 | // fixed |
---|
266 | |
---|
267 | struct dxJointFixed : public dxJoint { |
---|
268 | dQuaternion qrel; // initial relative rotation body1 -> body2 |
---|
269 | dVector3 offset; // relative offset between the bodies |
---|
270 | }; |
---|
271 | extern struct dxJoint::Vtable __dfixed_vtable; |
---|
272 | |
---|
273 | |
---|
274 | // null joint, for testing only |
---|
275 | |
---|
276 | struct dxJointNull : public dxJoint { |
---|
277 | }; |
---|
278 | extern struct dxJoint::Vtable __dnull_vtable; |
---|
279 | |
---|
280 | |
---|
281 | |
---|
282 | #endif |
---|