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 | /* |
---|
24 | |
---|
25 | design note: the general principle for giving a joint the option of connecting |
---|
26 | to the static environment (i.e. the absolute frame) is to check the second |
---|
27 | body (joint->node[1].body), and if it is zero then behave as if its body |
---|
28 | transform is the identity. |
---|
29 | |
---|
30 | */ |
---|
31 | |
---|
32 | #include <ode/ode.h> |
---|
33 | #include <ode/odemath.h> |
---|
34 | #include <ode/rotation.h> |
---|
35 | #include <ode/matrix.h> |
---|
36 | #include "joint.h" |
---|
37 | |
---|
38 | //**************************************************************************** |
---|
39 | // externs |
---|
40 | |
---|
41 | // extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); |
---|
42 | // extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); |
---|
43 | |
---|
44 | //**************************************************************************** |
---|
45 | // utility |
---|
46 | |
---|
47 | // set three "ball-and-socket" rows in the constraint equation, and the |
---|
48 | // corresponding right hand side. |
---|
49 | |
---|
50 | static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, |
---|
51 | dVector3 anchor1, dVector3 anchor2) |
---|
52 | { |
---|
53 | // anchor points in global coordinates with respect to body PORs. |
---|
54 | dVector3 a1,a2; |
---|
55 | |
---|
56 | int s = info->rowskip; |
---|
57 | |
---|
58 | // set jacobian |
---|
59 | info->J1l[0] = 1; |
---|
60 | info->J1l[s+1] = 1; |
---|
61 | info->J1l[2*s+2] = 1; |
---|
62 | dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); |
---|
63 | dCROSSMAT (info->J1a,a1,s,-,+); |
---|
64 | if (joint->node[1].body) { |
---|
65 | info->J2l[0] = -1; |
---|
66 | info->J2l[s+1] = -1; |
---|
67 | info->J2l[2*s+2] = -1; |
---|
68 | dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); |
---|
69 | dCROSSMAT (info->J2a,a2,s,+,-); |
---|
70 | } |
---|
71 | |
---|
72 | // set right hand side |
---|
73 | dReal k = info->fps * info->erp; |
---|
74 | if (joint->node[1].body) { |
---|
75 | for (int j=0; j<3; j++) { |
---|
76 | info->c[j] = k * (a2[j] + joint->node[1].body->posr.pos[j] - |
---|
77 | a1[j] - joint->node[0].body->posr.pos[j]); |
---|
78 | } |
---|
79 | } |
---|
80 | else { |
---|
81 | for (int j=0; j<3; j++) { |
---|
82 | info->c[j] = k * (anchor2[j] - a1[j] - |
---|
83 | joint->node[0].body->posr.pos[j]); |
---|
84 | } |
---|
85 | } |
---|
86 | } |
---|
87 | |
---|
88 | |
---|
89 | // this is like setBall(), except that `axis' is a unit length vector |
---|
90 | // (in global coordinates) that should be used for the first jacobian |
---|
91 | // position row (the other two row vectors will be derived from this). |
---|
92 | // `erp1' is the erp value to use along the axis. |
---|
93 | |
---|
94 | static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, |
---|
95 | dVector3 anchor1, dVector3 anchor2, |
---|
96 | dVector3 axis, dReal erp1) |
---|
97 | { |
---|
98 | // anchor points in global coordinates with respect to body PORs. |
---|
99 | dVector3 a1,a2; |
---|
100 | |
---|
101 | int i,s = info->rowskip; |
---|
102 | |
---|
103 | // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], |
---|
104 | // [0 1 0] and [0 0 1], which makes everything much easier. |
---|
105 | dVector3 q1,q2; |
---|
106 | dPlaneSpace (axis,q1,q2); |
---|
107 | |
---|
108 | // set jacobian |
---|
109 | for (i=0; i<3; i++) info->J1l[i] = axis[i]; |
---|
110 | for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; |
---|
111 | for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; |
---|
112 | dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); |
---|
113 | dCROSS (info->J1a,=,a1,axis); |
---|
114 | dCROSS (info->J1a+s,=,a1,q1); |
---|
115 | dCROSS (info->J1a+2*s,=,a1,q2); |
---|
116 | if (joint->node[1].body) { |
---|
117 | for (i=0; i<3; i++) info->J2l[i] = -axis[i]; |
---|
118 | for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; |
---|
119 | for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; |
---|
120 | dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); |
---|
121 | dCROSS (info->J2a,= -,a2,axis); |
---|
122 | dCROSS (info->J2a+s,= -,a2,q1); |
---|
123 | dCROSS (info->J2a+2*s,= -,a2,q2); |
---|
124 | } |
---|
125 | |
---|
126 | // set right hand side - measure error along (axis,q1,q2) |
---|
127 | dReal k1 = info->fps * erp1; |
---|
128 | dReal k = info->fps * info->erp; |
---|
129 | |
---|
130 | for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i]; |
---|
131 | if (joint->node[1].body) { |
---|
132 | for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i]; |
---|
133 | info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); |
---|
134 | info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); |
---|
135 | info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); |
---|
136 | } |
---|
137 | else { |
---|
138 | info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); |
---|
139 | info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); |
---|
140 | info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); |
---|
141 | } |
---|
142 | } |
---|
143 | |
---|
144 | |
---|
145 | // set three orientation rows in the constraint equation, and the |
---|
146 | // corresponding right hand side. |
---|
147 | |
---|
148 | static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) |
---|
149 | { |
---|
150 | int s = info->rowskip; |
---|
151 | int start_index = start_row * s; |
---|
152 | |
---|
153 | // 3 rows to make body rotations equal |
---|
154 | info->J1a[start_index] = 1; |
---|
155 | info->J1a[start_index + s + 1] = 1; |
---|
156 | info->J1a[start_index + s*2+2] = 1; |
---|
157 | if (joint->node[1].body) { |
---|
158 | info->J2a[start_index] = -1; |
---|
159 | info->J2a[start_index + s+1] = -1; |
---|
160 | info->J2a[start_index + s*2+2] = -1; |
---|
161 | } |
---|
162 | |
---|
163 | // compute the right hand side. the first three elements will result in |
---|
164 | // relative angular velocity of the two bodies - this is set to bring them |
---|
165 | // back into alignment. the correcting angular velocity is |
---|
166 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
167 | // = (erp*fps) * theta |
---|
168 | // angular_velocity = |angular_velocity| * u |
---|
169 | // = (erp*fps) * theta * u |
---|
170 | // where rotation along unit length axis u by theta brings body 2's frame |
---|
171 | // to qrel with respect to body 1's frame. using a small angle approximation |
---|
172 | // for sin(), this gives |
---|
173 | // angular_velocity = (erp*fps) * 2 * v |
---|
174 | // where the quaternion of the relative rotation between the two bodies is |
---|
175 | // q = [cos(theta/2) sin(theta/2)*u] = [s v] |
---|
176 | |
---|
177 | // get qerr = relative rotation (rotation error) between two bodies |
---|
178 | dQuaternion qerr,e; |
---|
179 | if (joint->node[1].body) { |
---|
180 | dQuaternion qq; |
---|
181 | dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); |
---|
182 | dQMultiply2 (qerr,qq,qrel); |
---|
183 | } |
---|
184 | else { |
---|
185 | dQMultiply3 (qerr,joint->node[0].body->q,qrel); |
---|
186 | } |
---|
187 | if (qerr[0] < 0) { |
---|
188 | qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small |
---|
189 | qerr[2] = -qerr[2]; |
---|
190 | qerr[3] = -qerr[3]; |
---|
191 | } |
---|
192 | dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding! |
---|
193 | dReal k = info->fps * info->erp; |
---|
194 | info->c[start_row] = 2*k * e[0]; |
---|
195 | info->c[start_row+1] = 2*k * e[1]; |
---|
196 | info->c[start_row+2] = 2*k * e[2]; |
---|
197 | } |
---|
198 | |
---|
199 | |
---|
200 | // compute anchor points relative to bodies |
---|
201 | |
---|
202 | static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, |
---|
203 | dVector3 anchor1, dVector3 anchor2) |
---|
204 | { |
---|
205 | if (j->node[0].body) { |
---|
206 | dReal q[4]; |
---|
207 | q[0] = x - j->node[0].body->posr.pos[0]; |
---|
208 | q[1] = y - j->node[0].body->posr.pos[1]; |
---|
209 | q[2] = z - j->node[0].body->posr.pos[2]; |
---|
210 | q[3] = 0; |
---|
211 | dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q); |
---|
212 | if (j->node[1].body) { |
---|
213 | q[0] = x - j->node[1].body->posr.pos[0]; |
---|
214 | q[1] = y - j->node[1].body->posr.pos[1]; |
---|
215 | q[2] = z - j->node[1].body->posr.pos[2]; |
---|
216 | q[3] = 0; |
---|
217 | dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q); |
---|
218 | } |
---|
219 | else { |
---|
220 | anchor2[0] = x; |
---|
221 | anchor2[1] = y; |
---|
222 | anchor2[2] = z; |
---|
223 | } |
---|
224 | } |
---|
225 | anchor1[3] = 0; |
---|
226 | anchor2[3] = 0; |
---|
227 | } |
---|
228 | |
---|
229 | |
---|
230 | // compute axes relative to bodies. either axis1 or axis2 can be 0. |
---|
231 | |
---|
232 | static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, |
---|
233 | dVector3 axis1, dVector3 axis2) |
---|
234 | { |
---|
235 | if (j->node[0].body) { |
---|
236 | dReal q[4]; |
---|
237 | q[0] = x; |
---|
238 | q[1] = y; |
---|
239 | q[2] = z; |
---|
240 | q[3] = 0; |
---|
241 | dNormalize3 (q); |
---|
242 | if (axis1) { |
---|
243 | dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q); |
---|
244 | axis1[3] = 0; |
---|
245 | } |
---|
246 | if (axis2) { |
---|
247 | if (j->node[1].body) { |
---|
248 | dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q); |
---|
249 | } |
---|
250 | else { |
---|
251 | axis2[0] = x; |
---|
252 | axis2[1] = y; |
---|
253 | axis2[2] = z; |
---|
254 | } |
---|
255 | axis2[3] = 0; |
---|
256 | } |
---|
257 | } |
---|
258 | } |
---|
259 | |
---|
260 | |
---|
261 | static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) |
---|
262 | { |
---|
263 | if (j->node[0].body) { |
---|
264 | dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1); |
---|
265 | result[0] += j->node[0].body->posr.pos[0]; |
---|
266 | result[1] += j->node[0].body->posr.pos[1]; |
---|
267 | result[2] += j->node[0].body->posr.pos[2]; |
---|
268 | } |
---|
269 | } |
---|
270 | |
---|
271 | |
---|
272 | static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) |
---|
273 | { |
---|
274 | if (j->node[1].body) { |
---|
275 | dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2); |
---|
276 | result[0] += j->node[1].body->posr.pos[0]; |
---|
277 | result[1] += j->node[1].body->posr.pos[1]; |
---|
278 | result[2] += j->node[1].body->posr.pos[2]; |
---|
279 | } |
---|
280 | else { |
---|
281 | result[0] = anchor2[0]; |
---|
282 | result[1] = anchor2[1]; |
---|
283 | result[2] = anchor2[2]; |
---|
284 | } |
---|
285 | } |
---|
286 | |
---|
287 | |
---|
288 | static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) |
---|
289 | { |
---|
290 | if (j->node[0].body) { |
---|
291 | dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1); |
---|
292 | } |
---|
293 | } |
---|
294 | |
---|
295 | |
---|
296 | static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) |
---|
297 | { |
---|
298 | if (j->node[1].body) { |
---|
299 | dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2); |
---|
300 | } |
---|
301 | else { |
---|
302 | result[0] = axis2[0]; |
---|
303 | result[1] = axis2[1]; |
---|
304 | result[2] = axis2[2]; |
---|
305 | } |
---|
306 | } |
---|
307 | |
---|
308 | |
---|
309 | static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) |
---|
310 | { |
---|
311 | // the angle between the two bodies is extracted from the quaternion that |
---|
312 | // represents the relative rotation between them. recall that a quaternion |
---|
313 | // q is: |
---|
314 | // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] |
---|
315 | // where s is a scalar and v is a 3-vector. u is a unit length axis and |
---|
316 | // theta is a rotation along that axis. we can get theta/2 by: |
---|
317 | // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) |
---|
318 | // but we can't get sin(theta/2) directly, only its absolute value, i.e.: |
---|
319 | // |v| = |sin(theta/2)| * |u| |
---|
320 | // = |sin(theta/2)| |
---|
321 | // using this value will have a strange effect. recall that there are two |
---|
322 | // quaternion representations of a given rotation, q and -q. typically as |
---|
323 | // a body rotates along the axis it will go through a complete cycle using |
---|
324 | // one representation and then the next cycle will use the other |
---|
325 | // representation. this corresponds to u pointing in the direction of the |
---|
326 | // hinge axis and then in the opposite direction. the result is that theta |
---|
327 | // will appear to go "backwards" every other cycle. here is a fix: if u |
---|
328 | // points "away" from the direction of the hinge (motor) axis (i.e. more |
---|
329 | // than 90 degrees) then use -q instead of q. this represents the same |
---|
330 | // rotation, but results in the cos(theta/2) value being sign inverted. |
---|
331 | |
---|
332 | // extract the angle from the quaternion. cost2 = cos(theta/2), |
---|
333 | // sint2 = |sin(theta/2)| |
---|
334 | dReal cost2 = qrel[0]; |
---|
335 | dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); |
---|
336 | dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions |
---|
337 | (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis |
---|
338 | (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction |
---|
339 | |
---|
340 | // the angle we get will be between 0..2*pi, but we want to return angles |
---|
341 | // between -pi..pi |
---|
342 | if (theta > M_PI) theta -= 2*M_PI; |
---|
343 | |
---|
344 | // the angle we've just extracted has the wrong sign |
---|
345 | theta = -theta; |
---|
346 | |
---|
347 | return theta; |
---|
348 | } |
---|
349 | |
---|
350 | |
---|
351 | // given two bodies (body1,body2), the hinge axis that they are connected by |
---|
352 | // w.r.t. body1 (axis), and the initial relative orientation between them |
---|
353 | // (q_initial), return the relative rotation angle. the initial relative |
---|
354 | // orientation corresponds to an angle of zero. if body2 is 0 then measure the |
---|
355 | // angle between body1 and the static frame. |
---|
356 | // |
---|
357 | // this will not return the correct angle if the bodies rotate along any axis |
---|
358 | // other than the given hinge axis. |
---|
359 | |
---|
360 | static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, |
---|
361 | dQuaternion q_initial) |
---|
362 | { |
---|
363 | // get qrel = relative rotation between the two bodies |
---|
364 | dQuaternion qrel; |
---|
365 | if (body2) { |
---|
366 | dQuaternion qq; |
---|
367 | dQMultiply1 (qq,body1->q,body2->q); |
---|
368 | dQMultiply2 (qrel,qq,q_initial); |
---|
369 | } |
---|
370 | else { |
---|
371 | // pretend body2->q is the identity |
---|
372 | dQMultiply3 (qrel,body1->q,q_initial); |
---|
373 | } |
---|
374 | |
---|
375 | return getHingeAngleFromRelativeQuat (qrel,axis); |
---|
376 | } |
---|
377 | |
---|
378 | //**************************************************************************** |
---|
379 | // dxJointLimitMotor |
---|
380 | |
---|
381 | void dxJointLimitMotor::init (dxWorld *world) |
---|
382 | { |
---|
383 | vel = 0; |
---|
384 | fmax = 0; |
---|
385 | lostop = -dInfinity; |
---|
386 | histop = dInfinity; |
---|
387 | fudge_factor = 1; |
---|
388 | normal_cfm = world->global_cfm; |
---|
389 | stop_erp = world->global_erp; |
---|
390 | stop_cfm = world->global_cfm; |
---|
391 | bounce = 0; |
---|
392 | limit = 0; |
---|
393 | limit_err = 0; |
---|
394 | } |
---|
395 | |
---|
396 | |
---|
397 | void dxJointLimitMotor::set (int num, dReal value) |
---|
398 | { |
---|
399 | switch (num) { |
---|
400 | case dParamLoStop: |
---|
401 | lostop = value; |
---|
402 | break; |
---|
403 | case dParamHiStop: |
---|
404 | histop = value; |
---|
405 | break; |
---|
406 | case dParamVel: |
---|
407 | vel = value; |
---|
408 | break; |
---|
409 | case dParamFMax: |
---|
410 | if (value >= 0) fmax = value; |
---|
411 | break; |
---|
412 | case dParamFudgeFactor: |
---|
413 | if (value >= 0 && value <= 1) fudge_factor = value; |
---|
414 | break; |
---|
415 | case dParamBounce: |
---|
416 | bounce = value; |
---|
417 | break; |
---|
418 | case dParamCFM: |
---|
419 | normal_cfm = value; |
---|
420 | break; |
---|
421 | case dParamStopERP: |
---|
422 | stop_erp = value; |
---|
423 | break; |
---|
424 | case dParamStopCFM: |
---|
425 | stop_cfm = value; |
---|
426 | break; |
---|
427 | } |
---|
428 | } |
---|
429 | |
---|
430 | |
---|
431 | dReal dxJointLimitMotor::get (int num) |
---|
432 | { |
---|
433 | switch (num) { |
---|
434 | case dParamLoStop: return lostop; |
---|
435 | case dParamHiStop: return histop; |
---|
436 | case dParamVel: return vel; |
---|
437 | case dParamFMax: return fmax; |
---|
438 | case dParamFudgeFactor: return fudge_factor; |
---|
439 | case dParamBounce: return bounce; |
---|
440 | case dParamCFM: return normal_cfm; |
---|
441 | case dParamStopERP: return stop_erp; |
---|
442 | case dParamStopCFM: return stop_cfm; |
---|
443 | default: return 0; |
---|
444 | } |
---|
445 | } |
---|
446 | |
---|
447 | |
---|
448 | int dxJointLimitMotor::testRotationalLimit (dReal angle) |
---|
449 | { |
---|
450 | if (angle <= lostop) { |
---|
451 | limit = 1; |
---|
452 | limit_err = angle - lostop; |
---|
453 | return 1; |
---|
454 | } |
---|
455 | else if (angle >= histop) { |
---|
456 | limit = 2; |
---|
457 | limit_err = angle - histop; |
---|
458 | return 1; |
---|
459 | } |
---|
460 | else { |
---|
461 | limit = 0; |
---|
462 | return 0; |
---|
463 | } |
---|
464 | } |
---|
465 | |
---|
466 | |
---|
467 | int dxJointLimitMotor::addLimot (dxJoint *joint, |
---|
468 | dxJoint::Info2 *info, int row, |
---|
469 | dVector3 ax1, int rotational) |
---|
470 | { |
---|
471 | int srow = row * info->rowskip; |
---|
472 | |
---|
473 | // if the joint is powered, or has joint limits, add in the extra row |
---|
474 | int powered = fmax > 0; |
---|
475 | if (powered || limit) { |
---|
476 | dReal *J1 = rotational ? info->J1a : info->J1l; |
---|
477 | dReal *J2 = rotational ? info->J2a : info->J2l; |
---|
478 | |
---|
479 | J1[srow+0] = ax1[0]; |
---|
480 | J1[srow+1] = ax1[1]; |
---|
481 | J1[srow+2] = ax1[2]; |
---|
482 | if (joint->node[1].body) { |
---|
483 | J2[srow+0] = -ax1[0]; |
---|
484 | J2[srow+1] = -ax1[1]; |
---|
485 | J2[srow+2] = -ax1[2]; |
---|
486 | } |
---|
487 | |
---|
488 | // linear limot torque decoupling step: |
---|
489 | // |
---|
490 | // if this is a linear limot (e.g. from a slider), we have to be careful |
---|
491 | // that the linear constraint forces (+/- ax1) applied to the two bodies |
---|
492 | // do not create a torque couple. in other words, the points that the |
---|
493 | // constraint force is applied at must lie along the same ax1 axis. |
---|
494 | // a torque couple will result in powered or limited slider-jointed free |
---|
495 | // bodies from gaining angular momentum. |
---|
496 | // the solution used here is to apply the constraint forces at the point |
---|
497 | // halfway between the body centers. there is no penalty (other than an |
---|
498 | // extra tiny bit of computation) in doing this adjustment. note that we |
---|
499 | // only need to do this if the constraint connects two bodies. |
---|
500 | |
---|
501 | dVector3 ltd; // Linear Torque Decoupling vector (a torque) |
---|
502 | if (!rotational && joint->node[1].body) { |
---|
503 | dVector3 c; |
---|
504 | c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); |
---|
505 | c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); |
---|
506 | c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); |
---|
507 | dCROSS (ltd,=,c,ax1); |
---|
508 | info->J1a[srow+0] = ltd[0]; |
---|
509 | info->J1a[srow+1] = ltd[1]; |
---|
510 | info->J1a[srow+2] = ltd[2]; |
---|
511 | info->J2a[srow+0] = ltd[0]; |
---|
512 | info->J2a[srow+1] = ltd[1]; |
---|
513 | info->J2a[srow+2] = ltd[2]; |
---|
514 | } |
---|
515 | |
---|
516 | // if we're limited low and high simultaneously, the joint motor is |
---|
517 | // ineffective |
---|
518 | if (limit && (lostop == histop)) powered = 0; |
---|
519 | |
---|
520 | if (powered) { |
---|
521 | info->cfm[row] = normal_cfm; |
---|
522 | if (! limit) { |
---|
523 | info->c[row] = vel; |
---|
524 | info->lo[row] = -fmax; |
---|
525 | info->hi[row] = fmax; |
---|
526 | } |
---|
527 | else { |
---|
528 | // the joint is at a limit, AND is being powered. if the joint is |
---|
529 | // being powered into the limit then we apply the maximum motor force |
---|
530 | // in that direction, because the motor is working against the |
---|
531 | // immovable limit. if the joint is being powered away from the limit |
---|
532 | // then we have problems because actually we need *two* lcp |
---|
533 | // constraints to handle this case. so we fake it and apply some |
---|
534 | // fraction of the maximum force. the fraction to use can be set as |
---|
535 | // a fudge factor. |
---|
536 | |
---|
537 | dReal fm = fmax; |
---|
538 | if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; |
---|
539 | |
---|
540 | // if we're powering away from the limit, apply the fudge factor |
---|
541 | if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; |
---|
542 | |
---|
543 | if (rotational) { |
---|
544 | dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], |
---|
545 | -fm*ax1[2]); |
---|
546 | if (joint->node[1].body) |
---|
547 | dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); |
---|
548 | } |
---|
549 | else { |
---|
550 | dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); |
---|
551 | if (joint->node[1].body) { |
---|
552 | dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); |
---|
553 | |
---|
554 | // linear limot torque decoupling step: refer to above discussion |
---|
555 | dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], |
---|
556 | -fm*ltd[2]); |
---|
557 | dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], |
---|
558 | -fm*ltd[2]); |
---|
559 | } |
---|
560 | } |
---|
561 | } |
---|
562 | } |
---|
563 | |
---|
564 | if (limit) { |
---|
565 | dReal k = info->fps * stop_erp; |
---|
566 | info->c[row] = -k * limit_err; |
---|
567 | info->cfm[row] = stop_cfm; |
---|
568 | |
---|
569 | if (lostop == histop) { |
---|
570 | // limited low and high simultaneously |
---|
571 | info->lo[row] = -dInfinity; |
---|
572 | info->hi[row] = dInfinity; |
---|
573 | } |
---|
574 | else { |
---|
575 | if (limit == 1) { |
---|
576 | // low limit |
---|
577 | info->lo[row] = 0; |
---|
578 | info->hi[row] = dInfinity; |
---|
579 | } |
---|
580 | else { |
---|
581 | // high limit |
---|
582 | info->lo[row] = -dInfinity; |
---|
583 | info->hi[row] = 0; |
---|
584 | } |
---|
585 | |
---|
586 | // deal with bounce |
---|
587 | if (bounce > 0) { |
---|
588 | // calculate joint velocity |
---|
589 | dReal vel; |
---|
590 | if (rotational) { |
---|
591 | vel = dDOT(joint->node[0].body->avel,ax1); |
---|
592 | if (joint->node[1].body) |
---|
593 | vel -= dDOT(joint->node[1].body->avel,ax1); |
---|
594 | } |
---|
595 | else { |
---|
596 | vel = dDOT(joint->node[0].body->lvel,ax1); |
---|
597 | if (joint->node[1].body) |
---|
598 | vel -= dDOT(joint->node[1].body->lvel,ax1); |
---|
599 | } |
---|
600 | |
---|
601 | // only apply bounce if the velocity is incoming, and if the |
---|
602 | // resulting c[] exceeds what we already have. |
---|
603 | if (limit == 1) { |
---|
604 | // low limit |
---|
605 | if (vel < 0) { |
---|
606 | dReal newc = -bounce * vel; |
---|
607 | if (newc > info->c[row]) info->c[row] = newc; |
---|
608 | } |
---|
609 | } |
---|
610 | else { |
---|
611 | // high limit - all those computations are reversed |
---|
612 | if (vel > 0) { |
---|
613 | dReal newc = -bounce * vel; |
---|
614 | if (newc < info->c[row]) info->c[row] = newc; |
---|
615 | } |
---|
616 | } |
---|
617 | } |
---|
618 | } |
---|
619 | } |
---|
620 | return 1; |
---|
621 | } |
---|
622 | else return 0; |
---|
623 | } |
---|
624 | |
---|
625 | //**************************************************************************** |
---|
626 | // ball and socket |
---|
627 | |
---|
628 | static void ballInit (dxJointBall *j) |
---|
629 | { |
---|
630 | dSetZero (j->anchor1,4); |
---|
631 | dSetZero (j->anchor2,4); |
---|
632 | j->erp = j->world->global_erp; |
---|
633 | j->cfm = j->world->global_cfm; |
---|
634 | } |
---|
635 | |
---|
636 | |
---|
637 | static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) |
---|
638 | { |
---|
639 | info->m = 3; |
---|
640 | info->nub = 3; |
---|
641 | } |
---|
642 | |
---|
643 | |
---|
644 | static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) |
---|
645 | { |
---|
646 | info->erp = joint->erp; |
---|
647 | info->cfm[0] = joint->cfm; |
---|
648 | info->cfm[1] = joint->cfm; |
---|
649 | info->cfm[2] = joint->cfm; |
---|
650 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
651 | } |
---|
652 | |
---|
653 | |
---|
654 | void dJointSetBallAnchor (dJointID j, dReal x, dReal y, dReal z) |
---|
655 | { |
---|
656 | dxJointBall* joint = (dxJointBall*)j; |
---|
657 | dUASSERT(joint,"bad joint argument"); |
---|
658 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
659 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
660 | } |
---|
661 | |
---|
662 | |
---|
663 | void dJointSetBallAnchor2 (dJointID j, dReal x, dReal y, dReal z) |
---|
664 | { |
---|
665 | dxJointBall* joint = (dxJointBall*)j; |
---|
666 | dUASSERT(joint,"bad joint argument"); |
---|
667 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
668 | joint->anchor2[0] = x; |
---|
669 | joint->anchor2[1] = y; |
---|
670 | joint->anchor2[2] = z; |
---|
671 | joint->anchor2[3] = 0; |
---|
672 | |
---|
673 | } |
---|
674 | |
---|
675 | void dJointGetBallAnchor (dJointID j, dVector3 result) |
---|
676 | { |
---|
677 | dxJointBall* joint = (dxJointBall*)j; |
---|
678 | dUASSERT(joint,"bad joint argument"); |
---|
679 | dUASSERT(result,"bad result argument"); |
---|
680 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
681 | if (joint->flags & dJOINT_REVERSE) |
---|
682 | getAnchor2 (joint,result,joint->anchor2); |
---|
683 | else |
---|
684 | getAnchor (joint,result,joint->anchor1); |
---|
685 | } |
---|
686 | |
---|
687 | |
---|
688 | void dJointGetBallAnchor2 (dJointID j, dVector3 result) |
---|
689 | { |
---|
690 | dxJointBall* joint = (dxJointBall*)j; |
---|
691 | dUASSERT(joint,"bad joint argument"); |
---|
692 | dUASSERT(result,"bad result argument"); |
---|
693 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
694 | if (joint->flags & dJOINT_REVERSE) |
---|
695 | getAnchor (joint,result,joint->anchor1); |
---|
696 | else |
---|
697 | getAnchor2 (joint,result,joint->anchor2); |
---|
698 | } |
---|
699 | |
---|
700 | |
---|
701 | void dxJointBall::set (int num, dReal value) |
---|
702 | { |
---|
703 | switch (num) { |
---|
704 | case dParamCFM: |
---|
705 | cfm = value; |
---|
706 | break; |
---|
707 | case dParamERP: |
---|
708 | erp = value; |
---|
709 | break; |
---|
710 | } |
---|
711 | } |
---|
712 | |
---|
713 | |
---|
714 | dReal dxJointBall::get (int num) |
---|
715 | { |
---|
716 | switch (num) { |
---|
717 | case dParamCFM: |
---|
718 | return cfm; |
---|
719 | case dParamERP: |
---|
720 | return erp; |
---|
721 | default: |
---|
722 | return 0; |
---|
723 | } |
---|
724 | } |
---|
725 | |
---|
726 | |
---|
727 | void dJointSetBallParam (dJointID j, int parameter, dReal value) |
---|
728 | { |
---|
729 | dxJointBall* joint = (dxJointBall*)j; |
---|
730 | dUASSERT(joint,"bad joint argument"); |
---|
731 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint"); |
---|
732 | joint->set (parameter,value); |
---|
733 | } |
---|
734 | |
---|
735 | |
---|
736 | dReal dJointGetBallParam (dJointID j, int parameter) |
---|
737 | { |
---|
738 | dxJointBall* joint = (dxJointBall*)j; |
---|
739 | dUASSERT(joint,"bad joint argument"); |
---|
740 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint"); |
---|
741 | return joint->get (parameter); |
---|
742 | } |
---|
743 | |
---|
744 | |
---|
745 | dxJoint::Vtable __dball_vtable = { |
---|
746 | sizeof(dxJointBall), |
---|
747 | (dxJoint::init_fn*) ballInit, |
---|
748 | (dxJoint::getInfo1_fn*) ballGetInfo1, |
---|
749 | (dxJoint::getInfo2_fn*) ballGetInfo2, |
---|
750 | dJointTypeBall}; |
---|
751 | |
---|
752 | //**************************************************************************** |
---|
753 | // hinge |
---|
754 | |
---|
755 | static void hingeInit (dxJointHinge *j) |
---|
756 | { |
---|
757 | dSetZero (j->anchor1,4); |
---|
758 | dSetZero (j->anchor2,4); |
---|
759 | dSetZero (j->axis1,4); |
---|
760 | j->axis1[0] = 1; |
---|
761 | dSetZero (j->axis2,4); |
---|
762 | j->axis2[0] = 1; |
---|
763 | dSetZero (j->qrel,4); |
---|
764 | j->limot.init (j->world); |
---|
765 | } |
---|
766 | |
---|
767 | |
---|
768 | static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) |
---|
769 | { |
---|
770 | info->nub = 5; |
---|
771 | |
---|
772 | // see if joint is powered |
---|
773 | if (j->limot.fmax > 0) |
---|
774 | info->m = 6; // powered hinge needs an extra constraint row |
---|
775 | else info->m = 5; |
---|
776 | |
---|
777 | // see if we're at a joint limit. |
---|
778 | if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && |
---|
779 | j->limot.lostop <= j->limot.histop) { |
---|
780 | dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, |
---|
781 | j->qrel); |
---|
782 | if (j->limot.testRotationalLimit (angle)) info->m = 6; |
---|
783 | } |
---|
784 | } |
---|
785 | |
---|
786 | |
---|
787 | static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) |
---|
788 | { |
---|
789 | // set the three ball-and-socket rows |
---|
790 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
791 | |
---|
792 | // set the two hinge rows. the hinge axis should be the only unconstrained |
---|
793 | // rotational axis, the angular velocity of the two bodies perpendicular to |
---|
794 | // the hinge axis should be equal. thus the constraint equations are |
---|
795 | // p*w1 - p*w2 = 0 |
---|
796 | // q*w1 - q*w2 = 0 |
---|
797 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
---|
798 | // are the angular velocity vectors of the two bodies. |
---|
799 | |
---|
800 | dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body |
---|
801 | dVector3 p,q; // plane space vectors for ax1 |
---|
802 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
---|
803 | dPlaneSpace (ax1,p,q); |
---|
804 | |
---|
805 | int s3=3*info->rowskip; |
---|
806 | int s4=4*info->rowskip; |
---|
807 | |
---|
808 | info->J1a[s3+0] = p[0]; |
---|
809 | info->J1a[s3+1] = p[1]; |
---|
810 | info->J1a[s3+2] = p[2]; |
---|
811 | info->J1a[s4+0] = q[0]; |
---|
812 | info->J1a[s4+1] = q[1]; |
---|
813 | info->J1a[s4+2] = q[2]; |
---|
814 | |
---|
815 | if (joint->node[1].body) { |
---|
816 | info->J2a[s3+0] = -p[0]; |
---|
817 | info->J2a[s3+1] = -p[1]; |
---|
818 | info->J2a[s3+2] = -p[2]; |
---|
819 | info->J2a[s4+0] = -q[0]; |
---|
820 | info->J2a[s4+1] = -q[1]; |
---|
821 | info->J2a[s4+2] = -q[2]; |
---|
822 | } |
---|
823 | |
---|
824 | // compute the right hand side of the constraint equation. set relative |
---|
825 | // body velocities along p and q to bring the hinge back into alignment. |
---|
826 | // if ax1,ax2 are the unit length hinge axes as computed from body1 and |
---|
827 | // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). |
---|
828 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
---|
829 | // along u to cover angle erp*theta in one step : |
---|
830 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
831 | // = (erp*fps) * theta |
---|
832 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
---|
833 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
---|
834 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
---|
835 | // theta ~= sin(theta), so |
---|
836 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
---|
837 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
---|
838 | // velocity to p and q to find the right hand side. |
---|
839 | |
---|
840 | dVector3 ax2,b; |
---|
841 | if (joint->node[1].body) { |
---|
842 | dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
---|
843 | } |
---|
844 | else { |
---|
845 | ax2[0] = joint->axis2[0]; |
---|
846 | ax2[1] = joint->axis2[1]; |
---|
847 | ax2[2] = joint->axis2[2]; |
---|
848 | } |
---|
849 | dCROSS (b,=,ax1,ax2); |
---|
850 | dReal k = info->fps * info->erp; |
---|
851 | info->c[3] = k * dDOT(b,p); |
---|
852 | info->c[4] = k * dDOT(b,q); |
---|
853 | |
---|
854 | // if the hinge is powered, or has joint limits, add in the stuff |
---|
855 | joint->limot.addLimot (joint,info,5,ax1,1); |
---|
856 | } |
---|
857 | |
---|
858 | |
---|
859 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
860 | |
---|
861 | static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) |
---|
862 | { |
---|
863 | if (joint->node[0].body) { |
---|
864 | if (joint->node[1].body) { |
---|
865 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
866 | } |
---|
867 | else { |
---|
868 | // set joint->qrel to the transpose of the first body q |
---|
869 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
870 | for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
871 | } |
---|
872 | } |
---|
873 | } |
---|
874 | |
---|
875 | |
---|
876 | void dJointSetHingeAnchor (dJointID j, dReal x, dReal y, dReal z) |
---|
877 | { |
---|
878 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
879 | dUASSERT(joint,"bad joint argument"); |
---|
880 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
881 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
882 | hingeComputeInitialRelativeRotation (joint); |
---|
883 | } |
---|
884 | |
---|
885 | |
---|
886 | void dJointSetHingeAnchorDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) |
---|
887 | { |
---|
888 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
889 | dUASSERT(joint,"bad joint argument"); |
---|
890 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
891 | |
---|
892 | if (joint->node[0].body) { |
---|
893 | dReal q[4]; |
---|
894 | q[0] = x - joint->node[0].body->posr.pos[0]; |
---|
895 | q[1] = y - joint->node[0].body->posr.pos[1]; |
---|
896 | q[2] = z - joint->node[0].body->posr.pos[2]; |
---|
897 | q[3] = 0; |
---|
898 | dMULTIPLY1_331 (joint->anchor1,joint->node[0].body->posr.R,q); |
---|
899 | |
---|
900 | if (joint->node[1].body) { |
---|
901 | q[0] = x - joint->node[1].body->posr.pos[0]; |
---|
902 | q[1] = y - joint->node[1].body->posr.pos[1]; |
---|
903 | q[2] = z - joint->node[1].body->posr.pos[2]; |
---|
904 | q[3] = 0; |
---|
905 | dMULTIPLY1_331 (joint->anchor2,joint->node[1].body->posr.R,q); |
---|
906 | } |
---|
907 | else { |
---|
908 | // Move the relative displacement between the passive body and the |
---|
909 | // anchor in the same direction as the passive body has just moved |
---|
910 | joint->anchor2[0] = x + dx; |
---|
911 | joint->anchor2[1] = y + dy; |
---|
912 | joint->anchor2[2] = z + dz; |
---|
913 | } |
---|
914 | } |
---|
915 | joint->anchor1[3] = 0; |
---|
916 | joint->anchor2[3] = 0; |
---|
917 | |
---|
918 | hingeComputeInitialRelativeRotation (joint); |
---|
919 | } |
---|
920 | |
---|
921 | |
---|
922 | |
---|
923 | void dJointSetHingeAxis (dJointID j, dReal x, dReal y, dReal z) |
---|
924 | { |
---|
925 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
926 | dUASSERT(joint,"bad joint argument"); |
---|
927 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
928 | setAxes (joint,x,y,z,joint->axis1,joint->axis2); |
---|
929 | hingeComputeInitialRelativeRotation (joint); |
---|
930 | } |
---|
931 | |
---|
932 | |
---|
933 | void dJointGetHingeAnchor (dJointID j, dVector3 result) |
---|
934 | { |
---|
935 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
936 | dUASSERT(joint,"bad joint argument"); |
---|
937 | dUASSERT(result,"bad result argument"); |
---|
938 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
939 | if (joint->flags & dJOINT_REVERSE) |
---|
940 | getAnchor2 (joint,result,joint->anchor2); |
---|
941 | else |
---|
942 | getAnchor (joint,result,joint->anchor1); |
---|
943 | } |
---|
944 | |
---|
945 | |
---|
946 | void dJointGetHingeAnchor2 (dJointID j, dVector3 result) |
---|
947 | { |
---|
948 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
949 | dUASSERT(joint,"bad joint argument"); |
---|
950 | dUASSERT(result,"bad result argument"); |
---|
951 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
952 | if (joint->flags & dJOINT_REVERSE) |
---|
953 | getAnchor (joint,result,joint->anchor1); |
---|
954 | else |
---|
955 | getAnchor2 (joint,result,joint->anchor2); |
---|
956 | } |
---|
957 | |
---|
958 | |
---|
959 | void dJointGetHingeAxis (dJointID j, dVector3 result) |
---|
960 | { |
---|
961 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
962 | dUASSERT(joint,"bad joint argument"); |
---|
963 | dUASSERT(result,"bad result argument"); |
---|
964 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
965 | getAxis (joint,result,joint->axis1); |
---|
966 | } |
---|
967 | |
---|
968 | |
---|
969 | void dJointSetHingeParam (dJointID j, int parameter, dReal value) |
---|
970 | { |
---|
971 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
972 | dUASSERT(joint,"bad joint argument"); |
---|
973 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
974 | joint->limot.set (parameter,value); |
---|
975 | } |
---|
976 | |
---|
977 | |
---|
978 | dReal dJointGetHingeParam (dJointID j, int parameter) |
---|
979 | { |
---|
980 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
981 | dUASSERT(joint,"bad joint argument"); |
---|
982 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
983 | return joint->limot.get (parameter); |
---|
984 | } |
---|
985 | |
---|
986 | |
---|
987 | dReal dJointGetHingeAngle (dJointID j) |
---|
988 | { |
---|
989 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
990 | dAASSERT(joint); |
---|
991 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
992 | if (joint->node[0].body) { |
---|
993 | dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, |
---|
994 | joint->qrel); |
---|
995 | if (joint->flags & dJOINT_REVERSE) |
---|
996 | return -ang; |
---|
997 | else |
---|
998 | return ang; |
---|
999 | } |
---|
1000 | else return 0; |
---|
1001 | } |
---|
1002 | |
---|
1003 | |
---|
1004 | dReal dJointGetHingeAngleRate (dJointID j) |
---|
1005 | { |
---|
1006 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
1007 | dAASSERT(joint); |
---|
1008 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); |
---|
1009 | if (joint->node[0].body) { |
---|
1010 | dVector3 axis; |
---|
1011 | dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); |
---|
1012 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
1013 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
1014 | if (joint->flags & dJOINT_REVERSE) rate = - rate; |
---|
1015 | return rate; |
---|
1016 | } |
---|
1017 | else return 0; |
---|
1018 | } |
---|
1019 | |
---|
1020 | |
---|
1021 | void dJointAddHingeTorque (dJointID j, dReal torque) |
---|
1022 | { |
---|
1023 | dxJointHinge* joint = (dxJointHinge*)j; |
---|
1024 | dVector3 axis; |
---|
1025 | dAASSERT(joint); |
---|
1026 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); |
---|
1027 | |
---|
1028 | if (joint->flags & dJOINT_REVERSE) |
---|
1029 | torque = -torque; |
---|
1030 | |
---|
1031 | getAxis (joint,axis,joint->axis1); |
---|
1032 | axis[0] *= torque; |
---|
1033 | axis[1] *= torque; |
---|
1034 | axis[2] *= torque; |
---|
1035 | |
---|
1036 | if (joint->node[0].body != 0) |
---|
1037 | dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); |
---|
1038 | if (joint->node[1].body != 0) |
---|
1039 | dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
---|
1040 | } |
---|
1041 | |
---|
1042 | |
---|
1043 | dxJoint::Vtable __dhinge_vtable = { |
---|
1044 | sizeof(dxJointHinge), |
---|
1045 | (dxJoint::init_fn*) hingeInit, |
---|
1046 | (dxJoint::getInfo1_fn*) hingeGetInfo1, |
---|
1047 | (dxJoint::getInfo2_fn*) hingeGetInfo2, |
---|
1048 | dJointTypeHinge}; |
---|
1049 | |
---|
1050 | //**************************************************************************** |
---|
1051 | // slider |
---|
1052 | |
---|
1053 | static void sliderInit (dxJointSlider *j) |
---|
1054 | { |
---|
1055 | dSetZero (j->axis1,4); |
---|
1056 | j->axis1[0] = 1; |
---|
1057 | dSetZero (j->qrel,4); |
---|
1058 | dSetZero (j->offset,4); |
---|
1059 | j->limot.init (j->world); |
---|
1060 | } |
---|
1061 | |
---|
1062 | |
---|
1063 | dReal dJointGetSliderPosition (dJointID j) |
---|
1064 | { |
---|
1065 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1066 | dUASSERT(joint,"bad joint argument"); |
---|
1067 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1068 | |
---|
1069 | // get axis1 in global coordinates |
---|
1070 | dVector3 ax1,q; |
---|
1071 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
---|
1072 | |
---|
1073 | if (joint->node[1].body) { |
---|
1074 | // get body2 + offset point in global coordinates |
---|
1075 | dMULTIPLY0_331 (q,joint->node[1].body->posr.R,joint->offset); |
---|
1076 | for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - q[i] - |
---|
1077 | joint->node[1].body->posr.pos[i]; |
---|
1078 | } |
---|
1079 | else { |
---|
1080 | for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - |
---|
1081 | joint->offset[i]; |
---|
1082 | |
---|
1083 | } |
---|
1084 | return dDOT(ax1,q); |
---|
1085 | } |
---|
1086 | |
---|
1087 | |
---|
1088 | dReal dJointGetSliderPositionRate (dJointID j) |
---|
1089 | { |
---|
1090 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1091 | dUASSERT(joint,"bad joint argument"); |
---|
1092 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1093 | |
---|
1094 | // get axis1 in global coordinates |
---|
1095 | dVector3 ax1; |
---|
1096 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
---|
1097 | |
---|
1098 | if (joint->node[1].body) { |
---|
1099 | return dDOT(ax1,joint->node[0].body->lvel) - |
---|
1100 | dDOT(ax1,joint->node[1].body->lvel); |
---|
1101 | } |
---|
1102 | else { |
---|
1103 | return dDOT(ax1,joint->node[0].body->lvel); |
---|
1104 | } |
---|
1105 | } |
---|
1106 | |
---|
1107 | |
---|
1108 | static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) |
---|
1109 | { |
---|
1110 | info->nub = 5; |
---|
1111 | |
---|
1112 | // see if joint is powered |
---|
1113 | if (j->limot.fmax > 0) |
---|
1114 | info->m = 6; // powered slider needs an extra constraint row |
---|
1115 | else info->m = 5; |
---|
1116 | |
---|
1117 | // see if we're at a joint limit. |
---|
1118 | j->limot.limit = 0; |
---|
1119 | if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && |
---|
1120 | j->limot.lostop <= j->limot.histop) { |
---|
1121 | // measure joint position |
---|
1122 | dReal pos = dJointGetSliderPosition (j); |
---|
1123 | if (pos <= j->limot.lostop) { |
---|
1124 | j->limot.limit = 1; |
---|
1125 | j->limot.limit_err = pos - j->limot.lostop; |
---|
1126 | info->m = 6; |
---|
1127 | } |
---|
1128 | else if (pos >= j->limot.histop) { |
---|
1129 | j->limot.limit = 2; |
---|
1130 | j->limot.limit_err = pos - j->limot.histop; |
---|
1131 | info->m = 6; |
---|
1132 | } |
---|
1133 | } |
---|
1134 | } |
---|
1135 | |
---|
1136 | |
---|
1137 | static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) |
---|
1138 | { |
---|
1139 | int i,s = info->rowskip; |
---|
1140 | int s3=3*s,s4=4*s; |
---|
1141 | |
---|
1142 | // pull out pos and R for both bodies. also get the `connection' |
---|
1143 | // vector pos2-pos1. |
---|
1144 | |
---|
1145 | dReal *pos1,*pos2,*R1,*R2; |
---|
1146 | dVector3 c; |
---|
1147 | pos1 = joint->node[0].body->posr.pos; |
---|
1148 | R1 = joint->node[0].body->posr.R; |
---|
1149 | if (joint->node[1].body) { |
---|
1150 | pos2 = joint->node[1].body->posr.pos; |
---|
1151 | R2 = joint->node[1].body->posr.R; |
---|
1152 | for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; |
---|
1153 | } |
---|
1154 | else { |
---|
1155 | pos2 = 0; |
---|
1156 | R2 = 0; |
---|
1157 | } |
---|
1158 | |
---|
1159 | // 3 rows to make body rotations equal |
---|
1160 | setFixedOrientation(joint, info, joint->qrel, 0); |
---|
1161 | |
---|
1162 | // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would |
---|
1163 | // result in three equations, so we project along the planespace vectors |
---|
1164 | // so that sliding along the slider axis is disregarded. for symmetry we |
---|
1165 | // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. |
---|
1166 | |
---|
1167 | dVector3 ax1; // joint axis in global coordinates (unit length) |
---|
1168 | dVector3 p,q; // plane space of ax1 |
---|
1169 | dMULTIPLY0_331 (ax1,R1,joint->axis1); |
---|
1170 | dPlaneSpace (ax1,p,q); |
---|
1171 | if (joint->node[1].body) { |
---|
1172 | dVector3 tmp; |
---|
1173 | dCROSS (tmp, = REAL(0.5) * ,c,p); |
---|
1174 | for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; |
---|
1175 | for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; |
---|
1176 | dCROSS (tmp, = REAL(0.5) * ,c,q); |
---|
1177 | for (i=0; i<3; i++) info->J1a[s4+i] = tmp[i]; |
---|
1178 | for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; |
---|
1179 | for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; |
---|
1180 | for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; |
---|
1181 | } |
---|
1182 | for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; |
---|
1183 | for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; |
---|
1184 | |
---|
1185 | // compute last two elements of right hand side. we want to align the offset |
---|
1186 | // point (in body 2's frame) with the center of body 1. |
---|
1187 | dReal k = info->fps * info->erp; |
---|
1188 | if (joint->node[1].body) { |
---|
1189 | dVector3 ofs; // offset point in global coordinates |
---|
1190 | dMULTIPLY0_331 (ofs,R2,joint->offset); |
---|
1191 | for (i=0; i<3; i++) c[i] += ofs[i]; |
---|
1192 | info->c[3] = k * dDOT(p,c); |
---|
1193 | info->c[4] = k * dDOT(q,c); |
---|
1194 | } |
---|
1195 | else { |
---|
1196 | dVector3 ofs; // offset point in global coordinates |
---|
1197 | for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; |
---|
1198 | info->c[3] = k * dDOT(p,ofs); |
---|
1199 | info->c[4] = k * dDOT(q,ofs); |
---|
1200 | } |
---|
1201 | |
---|
1202 | // if the slider is powered, or has joint limits, add in the extra row |
---|
1203 | joint->limot.addLimot (joint,info,5,ax1,0); |
---|
1204 | } |
---|
1205 | |
---|
1206 | |
---|
1207 | void dJointSetSliderAxis (dJointID j, dReal x, dReal y, dReal z) |
---|
1208 | { |
---|
1209 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1210 | int i; |
---|
1211 | dUASSERT(joint,"bad joint argument"); |
---|
1212 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1213 | setAxes (joint,x,y,z,joint->axis1,0); |
---|
1214 | |
---|
1215 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
1216 | // also compute center of body1 w.r.t body 2 |
---|
1217 | if (joint->node[1].body) { |
---|
1218 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
1219 | dVector3 c; |
---|
1220 | for (i=0; i<3; i++) |
---|
1221 | c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; |
---|
1222 | dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); |
---|
1223 | } |
---|
1224 | else { |
---|
1225 | // set joint->qrel to the transpose of the first body's q |
---|
1226 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
1227 | for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
1228 | for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; |
---|
1229 | } |
---|
1230 | } |
---|
1231 | |
---|
1232 | |
---|
1233 | void dJointSetSliderAxisDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) |
---|
1234 | { |
---|
1235 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1236 | int i; |
---|
1237 | dUASSERT(joint,"bad joint argument"); |
---|
1238 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1239 | setAxes (joint,x,y,z,joint->axis1,0); |
---|
1240 | |
---|
1241 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
1242 | // also compute center of body1 w.r.t body 2 |
---|
1243 | if (joint->node[1].body) { |
---|
1244 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
1245 | dVector3 c; |
---|
1246 | for (i=0; i<3; i++) |
---|
1247 | c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; |
---|
1248 | dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); |
---|
1249 | } |
---|
1250 | else { |
---|
1251 | // set joint->qrel to the transpose of the first body's q |
---|
1252 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
1253 | |
---|
1254 | for (i=1; i<4; i++) |
---|
1255 | joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
1256 | |
---|
1257 | joint->offset[0] = joint->node[0].body->posr.pos[0] + dx; |
---|
1258 | joint->offset[1] = joint->node[0].body->posr.pos[1] + dy; |
---|
1259 | joint->offset[2] = joint->node[0].body->posr.pos[2] + dz; |
---|
1260 | } |
---|
1261 | } |
---|
1262 | |
---|
1263 | |
---|
1264 | |
---|
1265 | void dJointGetSliderAxis (dJointID j, dVector3 result) |
---|
1266 | { |
---|
1267 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1268 | dUASSERT(joint,"bad joint argument"); |
---|
1269 | dUASSERT(result,"bad result argument"); |
---|
1270 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1271 | getAxis (joint,result,joint->axis1); |
---|
1272 | } |
---|
1273 | |
---|
1274 | |
---|
1275 | void dJointSetSliderParam (dJointID j, int parameter, dReal value) |
---|
1276 | { |
---|
1277 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1278 | dUASSERT(joint,"bad joint argument"); |
---|
1279 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1280 | joint->limot.set (parameter,value); |
---|
1281 | } |
---|
1282 | |
---|
1283 | |
---|
1284 | dReal dJointGetSliderParam (dJointID j, int parameter) |
---|
1285 | { |
---|
1286 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1287 | dUASSERT(joint,"bad joint argument"); |
---|
1288 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1289 | return joint->limot.get (parameter); |
---|
1290 | } |
---|
1291 | |
---|
1292 | |
---|
1293 | void dJointAddSliderForce (dJointID j, dReal force) |
---|
1294 | { |
---|
1295 | dxJointSlider* joint = (dxJointSlider*)j; |
---|
1296 | dVector3 axis; |
---|
1297 | dUASSERT(joint,"bad joint argument"); |
---|
1298 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1299 | |
---|
1300 | if (joint->flags & dJOINT_REVERSE) |
---|
1301 | force -= force; |
---|
1302 | |
---|
1303 | getAxis (joint,axis,joint->axis1); |
---|
1304 | axis[0] *= force; |
---|
1305 | axis[1] *= force; |
---|
1306 | axis[2] *= force; |
---|
1307 | |
---|
1308 | if (joint->node[0].body != 0) |
---|
1309 | dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); |
---|
1310 | if (joint->node[1].body != 0) |
---|
1311 | dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
---|
1312 | |
---|
1313 | if (joint->node[0].body != 0 && joint->node[1].body != 0) { |
---|
1314 | // linear torque decoupling: |
---|
1315 | // we have to compensate the torque, that this slider force may generate |
---|
1316 | // if body centers are not aligned along the slider axis |
---|
1317 | |
---|
1318 | dVector3 ltd; // Linear Torque Decoupling vector (a torque) |
---|
1319 | |
---|
1320 | dVector3 c; |
---|
1321 | c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); |
---|
1322 | c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); |
---|
1323 | c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); |
---|
1324 | dCROSS (ltd,=,c,axis); |
---|
1325 | |
---|
1326 | dBodyAddTorque (joint->node[0].body,ltd[0],ltd[1], ltd[2]); |
---|
1327 | dBodyAddTorque (joint->node[1].body,ltd[0],ltd[1], ltd[2]); |
---|
1328 | } |
---|
1329 | } |
---|
1330 | |
---|
1331 | |
---|
1332 | dxJoint::Vtable __dslider_vtable = { |
---|
1333 | sizeof(dxJointSlider), |
---|
1334 | (dxJoint::init_fn*) sliderInit, |
---|
1335 | (dxJoint::getInfo1_fn*) sliderGetInfo1, |
---|
1336 | (dxJoint::getInfo2_fn*) sliderGetInfo2, |
---|
1337 | dJointTypeSlider}; |
---|
1338 | |
---|
1339 | //**************************************************************************** |
---|
1340 | // contact |
---|
1341 | |
---|
1342 | static void contactInit (dxJointContact *j) |
---|
1343 | { |
---|
1344 | // default frictionless contact. hmmm, this info gets overwritten straight |
---|
1345 | // away anyway, so why bother? |
---|
1346 | #if 0 /* so don't bother ;) */ |
---|
1347 | j->contact.surface.mode = 0; |
---|
1348 | j->contact.surface.mu = 0; |
---|
1349 | dSetZero (j->contact.geom.pos,4); |
---|
1350 | dSetZero (j->contact.geom.normal,4); |
---|
1351 | j->contact.geom.depth = 0; |
---|
1352 | #endif |
---|
1353 | } |
---|
1354 | |
---|
1355 | |
---|
1356 | static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) |
---|
1357 | { |
---|
1358 | // make sure mu's >= 0, then calculate number of constraint rows and number |
---|
1359 | // of unbounded rows. |
---|
1360 | int m = 1, nub=0; |
---|
1361 | if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; |
---|
1362 | if (j->contact.surface.mode & dContactMu2) { |
---|
1363 | if (j->contact.surface.mu > 0) m++; |
---|
1364 | if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; |
---|
1365 | if (j->contact.surface.mu2 > 0) m++; |
---|
1366 | if (j->contact.surface.mu == dInfinity) nub ++; |
---|
1367 | if (j->contact.surface.mu2 == dInfinity) nub ++; |
---|
1368 | } |
---|
1369 | else { |
---|
1370 | if (j->contact.surface.mu > 0) m += 2; |
---|
1371 | if (j->contact.surface.mu == dInfinity) nub += 2; |
---|
1372 | } |
---|
1373 | |
---|
1374 | j->the_m = m; |
---|
1375 | info->m = m; |
---|
1376 | info->nub = nub; |
---|
1377 | } |
---|
1378 | |
---|
1379 | |
---|
1380 | static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) |
---|
1381 | { |
---|
1382 | int s = info->rowskip; |
---|
1383 | int s2 = 2*s; |
---|
1384 | |
---|
1385 | // get normal, with sign adjusted for body1/body2 polarity |
---|
1386 | dVector3 normal; |
---|
1387 | if (j->flags & dJOINT_REVERSE) { |
---|
1388 | normal[0] = - j->contact.geom.normal[0]; |
---|
1389 | normal[1] = - j->contact.geom.normal[1]; |
---|
1390 | normal[2] = - j->contact.geom.normal[2]; |
---|
1391 | } |
---|
1392 | else { |
---|
1393 | normal[0] = j->contact.geom.normal[0]; |
---|
1394 | normal[1] = j->contact.geom.normal[1]; |
---|
1395 | normal[2] = j->contact.geom.normal[2]; |
---|
1396 | } |
---|
1397 | normal[3] = 0; // @@@ hmmm |
---|
1398 | |
---|
1399 | // c1,c2 = contact points with respect to body PORs |
---|
1400 | dVector3 c1,c2; |
---|
1401 | c1[0] = j->contact.geom.pos[0] - j->node[0].body->posr.pos[0]; |
---|
1402 | c1[1] = j->contact.geom.pos[1] - j->node[0].body->posr.pos[1]; |
---|
1403 | c1[2] = j->contact.geom.pos[2] - j->node[0].body->posr.pos[2]; |
---|
1404 | |
---|
1405 | // set jacobian for normal |
---|
1406 | info->J1l[0] = normal[0]; |
---|
1407 | info->J1l[1] = normal[1]; |
---|
1408 | info->J1l[2] = normal[2]; |
---|
1409 | dCROSS (info->J1a,=,c1,normal); |
---|
1410 | if (j->node[1].body) { |
---|
1411 | c2[0] = j->contact.geom.pos[0] - j->node[1].body->posr.pos[0]; |
---|
1412 | c2[1] = j->contact.geom.pos[1] - j->node[1].body->posr.pos[1]; |
---|
1413 | c2[2] = j->contact.geom.pos[2] - j->node[1].body->posr.pos[2]; |
---|
1414 | info->J2l[0] = -normal[0]; |
---|
1415 | info->J2l[1] = -normal[1]; |
---|
1416 | info->J2l[2] = -normal[2]; |
---|
1417 | dCROSS (info->J2a,= -,c2,normal); |
---|
1418 | } |
---|
1419 | |
---|
1420 | // set right hand side and cfm value for normal |
---|
1421 | dReal erp = info->erp; |
---|
1422 | if (j->contact.surface.mode & dContactSoftERP) |
---|
1423 | erp = j->contact.surface.soft_erp; |
---|
1424 | dReal k = info->fps * erp; |
---|
1425 | dReal depth = j->contact.geom.depth - j->world->contactp.min_depth; |
---|
1426 | if (depth < 0) depth = 0; |
---|
1427 | |
---|
1428 | const dReal maxvel = j->world->contactp.max_vel; |
---|
1429 | info->c[0] = k*depth; |
---|
1430 | if (info->c[0] > maxvel) |
---|
1431 | info->c[0] = maxvel; |
---|
1432 | |
---|
1433 | if (j->contact.surface.mode & dContactSoftCFM) |
---|
1434 | info->cfm[0] = j->contact.surface.soft_cfm; |
---|
1435 | |
---|
1436 | // deal with bounce |
---|
1437 | if (j->contact.surface.mode & dContactBounce) { |
---|
1438 | // calculate outgoing velocity (-ve for incoming contact) |
---|
1439 | dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + |
---|
1440 | dDOT(info->J1a,j->node[0].body->avel); |
---|
1441 | if (j->node[1].body) { |
---|
1442 | outgoing += dDOT(info->J2l,j->node[1].body->lvel) + |
---|
1443 | dDOT(info->J2a,j->node[1].body->avel); |
---|
1444 | } |
---|
1445 | // only apply bounce if the outgoing velocity is greater than the |
---|
1446 | // threshold, and if the resulting c[0] exceeds what we already have. |
---|
1447 | if (j->contact.surface.bounce_vel >= 0 && |
---|
1448 | (-outgoing) > j->contact.surface.bounce_vel) { |
---|
1449 | dReal newc = - j->contact.surface.bounce * outgoing; |
---|
1450 | if (newc > info->c[0]) info->c[0] = newc; |
---|
1451 | } |
---|
1452 | } |
---|
1453 | |
---|
1454 | // set LCP limits for normal |
---|
1455 | info->lo[0] = 0; |
---|
1456 | info->hi[0] = dInfinity; |
---|
1457 | |
---|
1458 | // now do jacobian for tangential forces |
---|
1459 | dVector3 t1,t2; // two vectors tangential to normal |
---|
1460 | |
---|
1461 | // first friction direction |
---|
1462 | if (j->the_m >= 2) { |
---|
1463 | if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? |
---|
1464 | t1[0] = j->contact.fdir1[0]; |
---|
1465 | t1[1] = j->contact.fdir1[1]; |
---|
1466 | t1[2] = j->contact.fdir1[2]; |
---|
1467 | dCROSS (t2,=,normal,t1); |
---|
1468 | } |
---|
1469 | else { |
---|
1470 | dPlaneSpace (normal,t1,t2); |
---|
1471 | } |
---|
1472 | info->J1l[s+0] = t1[0]; |
---|
1473 | info->J1l[s+1] = t1[1]; |
---|
1474 | info->J1l[s+2] = t1[2]; |
---|
1475 | dCROSS (info->J1a+s,=,c1,t1); |
---|
1476 | if (j->node[1].body) { |
---|
1477 | info->J2l[s+0] = -t1[0]; |
---|
1478 | info->J2l[s+1] = -t1[1]; |
---|
1479 | info->J2l[s+2] = -t1[2]; |
---|
1480 | dCROSS (info->J2a+s,= -,c2,t1); |
---|
1481 | } |
---|
1482 | // set right hand side |
---|
1483 | if (j->contact.surface.mode & dContactMotion1) { |
---|
1484 | info->c[1] = j->contact.surface.motion1; |
---|
1485 | } |
---|
1486 | // set LCP bounds and friction index. this depends on the approximation |
---|
1487 | // mode |
---|
1488 | info->lo[1] = -j->contact.surface.mu; |
---|
1489 | info->hi[1] = j->contact.surface.mu; |
---|
1490 | if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; |
---|
1491 | |
---|
1492 | // set slip (constraint force mixing) |
---|
1493 | if (j->contact.surface.mode & dContactSlip1) |
---|
1494 | info->cfm[1] = j->contact.surface.slip1; |
---|
1495 | } |
---|
1496 | |
---|
1497 | // second friction direction |
---|
1498 | if (j->the_m >= 3) { |
---|
1499 | info->J1l[s2+0] = t2[0]; |
---|
1500 | info->J1l[s2+1] = t2[1]; |
---|
1501 | info->J1l[s2+2] = t2[2]; |
---|
1502 | dCROSS (info->J1a+s2,=,c1,t2); |
---|
1503 | if (j->node[1].body) { |
---|
1504 | info->J2l[s2+0] = -t2[0]; |
---|
1505 | info->J2l[s2+1] = -t2[1]; |
---|
1506 | info->J2l[s2+2] = -t2[2]; |
---|
1507 | dCROSS (info->J2a+s2,= -,c2,t2); |
---|
1508 | } |
---|
1509 | // set right hand side |
---|
1510 | if (j->contact.surface.mode & dContactMotion2) { |
---|
1511 | info->c[2] = j->contact.surface.motion2; |
---|
1512 | } |
---|
1513 | // set LCP bounds and friction index. this depends on the approximation |
---|
1514 | // mode |
---|
1515 | if (j->contact.surface.mode & dContactMu2) { |
---|
1516 | info->lo[2] = -j->contact.surface.mu2; |
---|
1517 | info->hi[2] = j->contact.surface.mu2; |
---|
1518 | } |
---|
1519 | else { |
---|
1520 | info->lo[2] = -j->contact.surface.mu; |
---|
1521 | info->hi[2] = j->contact.surface.mu; |
---|
1522 | } |
---|
1523 | if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; |
---|
1524 | |
---|
1525 | // set slip (constraint force mixing) |
---|
1526 | if (j->contact.surface.mode & dContactSlip2) |
---|
1527 | info->cfm[2] = j->contact.surface.slip2; |
---|
1528 | } |
---|
1529 | } |
---|
1530 | |
---|
1531 | |
---|
1532 | dxJoint::Vtable __dcontact_vtable = { |
---|
1533 | sizeof(dxJointContact), |
---|
1534 | (dxJoint::init_fn*) contactInit, |
---|
1535 | (dxJoint::getInfo1_fn*) contactGetInfo1, |
---|
1536 | (dxJoint::getInfo2_fn*) contactGetInfo2, |
---|
1537 | dJointTypeContact}; |
---|
1538 | |
---|
1539 | //**************************************************************************** |
---|
1540 | // hinge 2. note that this joint must be attached to two bodies for it to work |
---|
1541 | |
---|
1542 | static dReal measureHinge2Angle (dxJointHinge2 *joint) |
---|
1543 | { |
---|
1544 | dVector3 a1,a2; |
---|
1545 | dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2); |
---|
1546 | dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1); |
---|
1547 | dReal x = dDOT(joint->v1,a2); |
---|
1548 | dReal y = dDOT(joint->v2,a2); |
---|
1549 | return -dAtan2 (y,x); |
---|
1550 | } |
---|
1551 | |
---|
1552 | |
---|
1553 | static void hinge2Init (dxJointHinge2 *j) |
---|
1554 | { |
---|
1555 | dSetZero (j->anchor1,4); |
---|
1556 | dSetZero (j->anchor2,4); |
---|
1557 | dSetZero (j->axis1,4); |
---|
1558 | j->axis1[0] = 1; |
---|
1559 | dSetZero (j->axis2,4); |
---|
1560 | j->axis2[1] = 1; |
---|
1561 | j->c0 = 0; |
---|
1562 | j->s0 = 0; |
---|
1563 | |
---|
1564 | dSetZero (j->v1,4); |
---|
1565 | j->v1[0] = 1; |
---|
1566 | dSetZero (j->v2,4); |
---|
1567 | j->v2[1] = 1; |
---|
1568 | |
---|
1569 | j->limot1.init (j->world); |
---|
1570 | j->limot2.init (j->world); |
---|
1571 | |
---|
1572 | j->susp_erp = j->world->global_erp; |
---|
1573 | j->susp_cfm = j->world->global_cfm; |
---|
1574 | |
---|
1575 | j->flags |= dJOINT_TWOBODIES; |
---|
1576 | } |
---|
1577 | |
---|
1578 | |
---|
1579 | static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) |
---|
1580 | { |
---|
1581 | info->m = 4; |
---|
1582 | info->nub = 4; |
---|
1583 | |
---|
1584 | // see if we're powered or at a joint limit for axis 1 |
---|
1585 | int atlimit=0; |
---|
1586 | if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && |
---|
1587 | j->limot1.lostop <= j->limot1.histop) { |
---|
1588 | dReal angle = measureHinge2Angle (j); |
---|
1589 | if (j->limot1.testRotationalLimit (angle)) atlimit = 1; |
---|
1590 | } |
---|
1591 | if (atlimit || j->limot1.fmax > 0) info->m++; |
---|
1592 | |
---|
1593 | // see if we're powering axis 2 (we currently never limit this axis) |
---|
1594 | j->limot2.limit = 0; |
---|
1595 | if (j->limot2.fmax > 0) info->m++; |
---|
1596 | } |
---|
1597 | |
---|
1598 | |
---|
1599 | // macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are |
---|
1600 | // relative to body 1 and 2 initially) and then computes the constrained |
---|
1601 | // rotational axis as the cross product of ax1 and ax2. |
---|
1602 | // the sin and cos of the angle between axis 1 and 2 is computed, this comes |
---|
1603 | // from dot and cross product rules. |
---|
1604 | |
---|
1605 | #define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ |
---|
1606 | dVector3 ax1,ax2; \ |
---|
1607 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); \ |
---|
1608 | dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); \ |
---|
1609 | dCROSS (axis,=,ax1,ax2); \ |
---|
1610 | sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ |
---|
1611 | cos_angle = dDOT (ax1,ax2); |
---|
1612 | |
---|
1613 | |
---|
1614 | static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) |
---|
1615 | { |
---|
1616 | // get information we need to set the hinge row |
---|
1617 | dReal s,c; |
---|
1618 | dVector3 q; |
---|
1619 | HINGE2_GET_AXIS_INFO (q,s,c); |
---|
1620 | dNormalize3 (q); // @@@ quicker: divide q by s ? |
---|
1621 | |
---|
1622 | // set the three ball-and-socket rows (aligned to the suspension axis ax1) |
---|
1623 | setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); |
---|
1624 | |
---|
1625 | // set the hinge row |
---|
1626 | int s3=3*info->rowskip; |
---|
1627 | info->J1a[s3+0] = q[0]; |
---|
1628 | info->J1a[s3+1] = q[1]; |
---|
1629 | info->J1a[s3+2] = q[2]; |
---|
1630 | if (joint->node[1].body) { |
---|
1631 | info->J2a[s3+0] = -q[0]; |
---|
1632 | info->J2a[s3+1] = -q[1]; |
---|
1633 | info->J2a[s3+2] = -q[2]; |
---|
1634 | } |
---|
1635 | |
---|
1636 | // compute the right hand side for the constrained rotational DOF. |
---|
1637 | // axis 1 and axis 2 are separated by an angle `theta'. the desired |
---|
1638 | // separation angle is theta0. sin(theta0) and cos(theta0) are recorded |
---|
1639 | // in the joint structure. the correcting angular velocity is: |
---|
1640 | // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize |
---|
1641 | // = (erp*fps) * (theta0-theta) |
---|
1642 | // (theta0-theta) can be computed using the following small-angle-difference |
---|
1643 | // approximation: |
---|
1644 | // theta0-theta ~= tan(theta0-theta) |
---|
1645 | // = sin(theta0-theta)/cos(theta0-theta) |
---|
1646 | // = (c*s0 - s*c0) / (c*c0 + s*s0) |
---|
1647 | // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 |
---|
1648 | // where c = cos(theta), s = sin(theta) |
---|
1649 | // c0 = cos(theta0), s0 = sin(theta0) |
---|
1650 | |
---|
1651 | dReal k = info->fps * info->erp; |
---|
1652 | info->c[3] = k * (joint->c0 * s - joint->s0 * c); |
---|
1653 | |
---|
1654 | // if the axis1 hinge is powered, or has joint limits, add in more stuff |
---|
1655 | int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
---|
1656 | |
---|
1657 | // if the axis2 hinge is powered, add in more stuff |
---|
1658 | joint->limot2.addLimot (joint,info,row,ax2,1); |
---|
1659 | |
---|
1660 | // set parameter for the suspension |
---|
1661 | info->cfm[0] = joint->susp_cfm; |
---|
1662 | } |
---|
1663 | |
---|
1664 | |
---|
1665 | // compute vectors v1 and v2 (embedded in body1), used to measure angle |
---|
1666 | // between body 1 and body 2 |
---|
1667 | |
---|
1668 | static void makeHinge2V1andV2 (dxJointHinge2 *joint) |
---|
1669 | { |
---|
1670 | if (joint->node[0].body) { |
---|
1671 | // get axis 1 and 2 in global coords |
---|
1672 | dVector3 ax1,ax2,v; |
---|
1673 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
---|
1674 | dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
---|
1675 | |
---|
1676 | // don't do anything if the axis1 or axis2 vectors are zero or the same |
---|
1677 | if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || |
---|
1678 | (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || |
---|
1679 | (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; |
---|
1680 | |
---|
1681 | // modify axis 2 so it's perpendicular to axis 1 |
---|
1682 | dReal k = dDOT(ax1,ax2); |
---|
1683 | for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; |
---|
1684 | dNormalize3 (ax2); |
---|
1685 | |
---|
1686 | // make v1 = modified axis2, v2 = axis1 x (modified axis2) |
---|
1687 | dCROSS (v,=,ax1,ax2); |
---|
1688 | dMULTIPLY1_331 (joint->v1,joint->node[0].body->posr.R,ax2); |
---|
1689 | dMULTIPLY1_331 (joint->v2,joint->node[0].body->posr.R,v); |
---|
1690 | } |
---|
1691 | } |
---|
1692 | |
---|
1693 | |
---|
1694 | void dJointSetHinge2Anchor (dJointID j, dReal x, dReal y, dReal z) |
---|
1695 | { |
---|
1696 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1697 | dUASSERT(joint,"bad joint argument"); |
---|
1698 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1699 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
1700 | makeHinge2V1andV2 (joint); |
---|
1701 | } |
---|
1702 | |
---|
1703 | |
---|
1704 | void dJointSetHinge2Axis1 (dJointID j, dReal x, dReal y, dReal z) |
---|
1705 | { |
---|
1706 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1707 | dUASSERT(joint,"bad joint argument"); |
---|
1708 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1709 | if (joint->node[0].body) { |
---|
1710 | dReal q[4]; |
---|
1711 | q[0] = x; |
---|
1712 | q[1] = y; |
---|
1713 | q[2] = z; |
---|
1714 | q[3] = 0; |
---|
1715 | dNormalize3 (q); |
---|
1716 | dMULTIPLY1_331 (joint->axis1,joint->node[0].body->posr.R,q); |
---|
1717 | joint->axis1[3] = 0; |
---|
1718 | |
---|
1719 | // compute the sin and cos of the angle between axis 1 and axis 2 |
---|
1720 | dVector3 ax; |
---|
1721 | HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
---|
1722 | } |
---|
1723 | makeHinge2V1andV2 (joint); |
---|
1724 | } |
---|
1725 | |
---|
1726 | |
---|
1727 | void dJointSetHinge2Axis2 (dJointID j, dReal x, dReal y, dReal z) |
---|
1728 | { |
---|
1729 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1730 | dUASSERT(joint,"bad joint argument"); |
---|
1731 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1732 | if (joint->node[1].body) { |
---|
1733 | dReal q[4]; |
---|
1734 | q[0] = x; |
---|
1735 | q[1] = y; |
---|
1736 | q[2] = z; |
---|
1737 | q[3] = 0; |
---|
1738 | dNormalize3 (q); |
---|
1739 | dMULTIPLY1_331 (joint->axis2,joint->node[1].body->posr.R,q); |
---|
1740 | joint->axis1[3] = 0; |
---|
1741 | |
---|
1742 | // compute the sin and cos of the angle between axis 1 and axis 2 |
---|
1743 | dVector3 ax; |
---|
1744 | HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
---|
1745 | } |
---|
1746 | makeHinge2V1andV2 (joint); |
---|
1747 | } |
---|
1748 | |
---|
1749 | |
---|
1750 | void dJointSetHinge2Param (dJointID j, int parameter, dReal value) |
---|
1751 | { |
---|
1752 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1753 | dUASSERT(joint,"bad joint argument"); |
---|
1754 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1755 | if ((parameter & 0xff00) == 0x100) { |
---|
1756 | joint->limot2.set (parameter & 0xff,value); |
---|
1757 | } |
---|
1758 | else { |
---|
1759 | if (parameter == dParamSuspensionERP) joint->susp_erp = value; |
---|
1760 | else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; |
---|
1761 | else joint->limot1.set (parameter,value); |
---|
1762 | } |
---|
1763 | } |
---|
1764 | |
---|
1765 | |
---|
1766 | void dJointGetHinge2Anchor (dJointID j, dVector3 result) |
---|
1767 | { |
---|
1768 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1769 | dUASSERT(joint,"bad joint argument"); |
---|
1770 | dUASSERT(result,"bad result argument"); |
---|
1771 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1772 | if (joint->flags & dJOINT_REVERSE) |
---|
1773 | getAnchor2 (joint,result,joint->anchor2); |
---|
1774 | else |
---|
1775 | getAnchor (joint,result,joint->anchor1); |
---|
1776 | } |
---|
1777 | |
---|
1778 | |
---|
1779 | void dJointGetHinge2Anchor2 (dJointID j, dVector3 result) |
---|
1780 | { |
---|
1781 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1782 | dUASSERT(joint,"bad joint argument"); |
---|
1783 | dUASSERT(result,"bad result argument"); |
---|
1784 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1785 | if (joint->flags & dJOINT_REVERSE) |
---|
1786 | getAnchor (joint,result,joint->anchor1); |
---|
1787 | else |
---|
1788 | getAnchor2 (joint,result,joint->anchor2); |
---|
1789 | } |
---|
1790 | |
---|
1791 | |
---|
1792 | void dJointGetHinge2Axis1 (dJointID j, dVector3 result) |
---|
1793 | { |
---|
1794 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1795 | dUASSERT(joint,"bad joint argument"); |
---|
1796 | dUASSERT(result,"bad result argument"); |
---|
1797 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1798 | if (joint->node[0].body) { |
---|
1799 | dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis1); |
---|
1800 | } |
---|
1801 | } |
---|
1802 | |
---|
1803 | |
---|
1804 | void dJointGetHinge2Axis2 (dJointID j, dVector3 result) |
---|
1805 | { |
---|
1806 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1807 | dUASSERT(joint,"bad joint argument"); |
---|
1808 | dUASSERT(result,"bad result argument"); |
---|
1809 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1810 | if (joint->node[1].body) { |
---|
1811 | dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis2); |
---|
1812 | } |
---|
1813 | } |
---|
1814 | |
---|
1815 | |
---|
1816 | dReal dJointGetHinge2Param (dJointID j, int parameter) |
---|
1817 | { |
---|
1818 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1819 | dUASSERT(joint,"bad joint argument"); |
---|
1820 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1821 | if ((parameter & 0xff00) == 0x100) { |
---|
1822 | return joint->limot2.get (parameter & 0xff); |
---|
1823 | } |
---|
1824 | else { |
---|
1825 | if (parameter == dParamSuspensionERP) return joint->susp_erp; |
---|
1826 | else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; |
---|
1827 | else return joint->limot1.get (parameter); |
---|
1828 | } |
---|
1829 | } |
---|
1830 | |
---|
1831 | |
---|
1832 | dReal dJointGetHinge2Angle1 (dJointID j) |
---|
1833 | { |
---|
1834 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1835 | dUASSERT(joint,"bad joint argument"); |
---|
1836 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1837 | if (joint->node[0].body) return measureHinge2Angle (joint); |
---|
1838 | else return 0; |
---|
1839 | } |
---|
1840 | |
---|
1841 | |
---|
1842 | dReal dJointGetHinge2Angle1Rate (dJointID j) |
---|
1843 | { |
---|
1844 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1845 | dUASSERT(joint,"bad joint argument"); |
---|
1846 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1847 | if (joint->node[0].body) { |
---|
1848 | dVector3 axis; |
---|
1849 | dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); |
---|
1850 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
1851 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
1852 | return rate; |
---|
1853 | } |
---|
1854 | else return 0; |
---|
1855 | } |
---|
1856 | |
---|
1857 | |
---|
1858 | dReal dJointGetHinge2Angle2Rate (dJointID j) |
---|
1859 | { |
---|
1860 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1861 | dUASSERT(joint,"bad joint argument"); |
---|
1862 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1863 | if (joint->node[0].body && joint->node[1].body) { |
---|
1864 | dVector3 axis; |
---|
1865 | dMULTIPLY0_331 (axis,joint->node[1].body->posr.R,joint->axis2); |
---|
1866 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
1867 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
1868 | return rate; |
---|
1869 | } |
---|
1870 | else return 0; |
---|
1871 | } |
---|
1872 | |
---|
1873 | |
---|
1874 | void dJointAddHinge2Torques (dJointID j, dReal torque1, dReal torque2) |
---|
1875 | { |
---|
1876 | dxJointHinge2* joint = (dxJointHinge2*)j; |
---|
1877 | dVector3 axis1, axis2; |
---|
1878 | dUASSERT(joint,"bad joint argument"); |
---|
1879 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1880 | |
---|
1881 | if (joint->node[0].body && joint->node[1].body) { |
---|
1882 | dMULTIPLY0_331 (axis1,joint->node[0].body->posr.R,joint->axis1); |
---|
1883 | dMULTIPLY0_331 (axis2,joint->node[1].body->posr.R,joint->axis2); |
---|
1884 | axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; |
---|
1885 | axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; |
---|
1886 | axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; |
---|
1887 | dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
---|
1888 | dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
---|
1889 | } |
---|
1890 | } |
---|
1891 | |
---|
1892 | |
---|
1893 | dxJoint::Vtable __dhinge2_vtable = { |
---|
1894 | sizeof(dxJointHinge2), |
---|
1895 | (dxJoint::init_fn*) hinge2Init, |
---|
1896 | (dxJoint::getInfo1_fn*) hinge2GetInfo1, |
---|
1897 | (dxJoint::getInfo2_fn*) hinge2GetInfo2, |
---|
1898 | dJointTypeHinge2}; |
---|
1899 | |
---|
1900 | //**************************************************************************** |
---|
1901 | // universal |
---|
1902 | |
---|
1903 | // I just realized that the universal joint is equivalent to a hinge 2 joint with |
---|
1904 | // perfectly stiff suspension. By comparing the hinge 2 implementation to |
---|
1905 | // the universal implementation, you may be able to improve this |
---|
1906 | // implementation (or, less likely, the hinge2 implementation). |
---|
1907 | |
---|
1908 | static void universalInit (dxJointUniversal *j) |
---|
1909 | { |
---|
1910 | dSetZero (j->anchor1,4); |
---|
1911 | dSetZero (j->anchor2,4); |
---|
1912 | dSetZero (j->axis1,4); |
---|
1913 | j->axis1[0] = 1; |
---|
1914 | dSetZero (j->axis2,4); |
---|
1915 | j->axis2[1] = 1; |
---|
1916 | dSetZero(j->qrel1,4); |
---|
1917 | dSetZero(j->qrel2,4); |
---|
1918 | j->limot1.init (j->world); |
---|
1919 | j->limot2.init (j->world); |
---|
1920 | } |
---|
1921 | |
---|
1922 | |
---|
1923 | static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) |
---|
1924 | { |
---|
1925 | // This says "ax1 = joint->node[0].body->posr.R * joint->axis1" |
---|
1926 | dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
---|
1927 | |
---|
1928 | if (joint->node[1].body) { |
---|
1929 | dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
---|
1930 | } |
---|
1931 | else { |
---|
1932 | ax2[0] = joint->axis2[0]; |
---|
1933 | ax2[1] = joint->axis2[1]; |
---|
1934 | ax2[2] = joint->axis2[2]; |
---|
1935 | } |
---|
1936 | } |
---|
1937 | |
---|
1938 | static void getUniversalAngles(dxJointUniversal *joint, dReal *angle1, dReal *angle2) |
---|
1939 | { |
---|
1940 | if (joint->node[0].body) |
---|
1941 | { |
---|
1942 | // length 1 joint axis in global coordinates, from each body |
---|
1943 | dVector3 ax1, ax2; |
---|
1944 | dMatrix3 R; |
---|
1945 | dQuaternion qcross, qq, qrel; |
---|
1946 | |
---|
1947 | getUniversalAxes (joint,ax1,ax2); |
---|
1948 | |
---|
1949 | // It should be possible to get both angles without explicitly |
---|
1950 | // constructing the rotation matrix of the cross. Basically, |
---|
1951 | // orientation of the cross about axis1 comes from body 2, |
---|
1952 | // about axis 2 comes from body 1, and the perpendicular |
---|
1953 | // axis can come from the two bodies somehow. (We don't really |
---|
1954 | // want to assume it's 90 degrees, because in general the |
---|
1955 | // constraints won't be perfectly satisfied, or even very well |
---|
1956 | // satisfied.) |
---|
1957 | // |
---|
1958 | // However, we'd need a version of getHingeAngleFromRElativeQuat() |
---|
1959 | // that CAN handle when its relative quat is rotated along a direction |
---|
1960 | // other than the given axis. What I have here works, |
---|
1961 | // although it's probably much slower than need be. |
---|
1962 | |
---|
1963 | dRFrom2Axes (R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
---|
1964 | |
---|
1965 | dRtoQ (R, qcross); |
---|
1966 | |
---|
1967 | |
---|
1968 | // This code is essentialy the same as getHingeAngle(), see the comments |
---|
1969 | // there for details. |
---|
1970 | |
---|
1971 | // get qrel = relative rotation between node[0] and the cross |
---|
1972 | dQMultiply1 (qq, joint->node[0].body->q, qcross); |
---|
1973 | dQMultiply2 (qrel, qq, joint->qrel1); |
---|
1974 | |
---|
1975 | *angle1 = getHingeAngleFromRelativeQuat(qrel, joint->axis1); |
---|
1976 | |
---|
1977 | // This is equivalent to |
---|
1978 | // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
---|
1979 | // You see that the R is constructed from the same 2 axis as for angle1 |
---|
1980 | // but the first and second axis are swapped. |
---|
1981 | // So we can take the first R and rapply a rotation to it. |
---|
1982 | // The rotation is around the axis between the 2 axes (ax1 and ax2). |
---|
1983 | // We do a rotation of 180deg. |
---|
1984 | |
---|
1985 | dQuaternion qcross2; |
---|
1986 | // Find the vector between ax1 and ax2 (i.e. in the middle) |
---|
1987 | // We need to turn around this vector by 180deg |
---|
1988 | |
---|
1989 | // The 2 axes should be normalize so to find the vector between the 2. |
---|
1990 | // Add and devide by 2 then normalize or simply normalize |
---|
1991 | // ax2 |
---|
1992 | // ^ |
---|
1993 | // | |
---|
1994 | // | |
---|
1995 | /// *------------> ax1 |
---|
1996 | // We want the vector a 45deg |
---|
1997 | // |
---|
1998 | // N.B. We don't need to normalize the ax1 and ax2 since there are |
---|
1999 | // normalized when we set them. |
---|
2000 | |
---|
2001 | // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z] |
---|
2002 | qrel[0] = 0; // equivalent to cos(Pi/2) |
---|
2003 | qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1 |
---|
2004 | qrel[2] = ax1[1] + ax2[1]; |
---|
2005 | qrel[3] = ax1[2] + ax2[2]; |
---|
2006 | |
---|
2007 | dReal l = dRecip(sqrt(qrel[1]*qrel[1] + qrel[2]*qrel[2] + qrel[3]*qrel[3])); |
---|
2008 | qrel[1] *= l; |
---|
2009 | qrel[2] *= l; |
---|
2010 | qrel[3] *= l; |
---|
2011 | |
---|
2012 | dQMultiply0 (qcross2, qrel, qcross); |
---|
2013 | |
---|
2014 | if (joint->node[1].body) { |
---|
2015 | dQMultiply1 (qq, joint->node[1].body->q, qcross2); |
---|
2016 | dQMultiply2 (qrel, qq, joint->qrel2); |
---|
2017 | } |
---|
2018 | else { |
---|
2019 | // pretend joint->node[1].body->q is the identity |
---|
2020 | dQMultiply2 (qrel, qcross2, joint->qrel2); |
---|
2021 | } |
---|
2022 | |
---|
2023 | *angle2 = - getHingeAngleFromRelativeQuat(qrel, joint->axis2); |
---|
2024 | |
---|
2025 | } |
---|
2026 | else |
---|
2027 | { |
---|
2028 | *angle1 = 0; |
---|
2029 | *angle2 = 0; |
---|
2030 | } |
---|
2031 | } |
---|
2032 | |
---|
2033 | static dReal getUniversalAngle1(dxJointUniversal *joint) |
---|
2034 | { |
---|
2035 | if (joint->node[0].body) { |
---|
2036 | // length 1 joint axis in global coordinates, from each body |
---|
2037 | dVector3 ax1, ax2; |
---|
2038 | dMatrix3 R; |
---|
2039 | dQuaternion qcross, qq, qrel; |
---|
2040 | |
---|
2041 | getUniversalAxes (joint,ax1,ax2); |
---|
2042 | |
---|
2043 | // It should be possible to get both angles without explicitly |
---|
2044 | // constructing the rotation matrix of the cross. Basically, |
---|
2045 | // orientation of the cross about axis1 comes from body 2, |
---|
2046 | // about axis 2 comes from body 1, and the perpendicular |
---|
2047 | // axis can come from the two bodies somehow. (We don't really |
---|
2048 | // want to assume it's 90 degrees, because in general the |
---|
2049 | // constraints won't be perfectly satisfied, or even very well |
---|
2050 | // satisfied.) |
---|
2051 | // |
---|
2052 | // However, we'd need a version of getHingeAngleFromRElativeQuat() |
---|
2053 | // that CAN handle when its relative quat is rotated along a direction |
---|
2054 | // other than the given axis. What I have here works, |
---|
2055 | // although it's probably much slower than need be. |
---|
2056 | |
---|
2057 | dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
---|
2058 | dRtoQ (R,qcross); |
---|
2059 | |
---|
2060 | // This code is essential the same as getHingeAngle(), see the comments |
---|
2061 | // there for details. |
---|
2062 | |
---|
2063 | // get qrel = relative rotation between node[0] and the cross |
---|
2064 | dQMultiply1 (qq,joint->node[0].body->q,qcross); |
---|
2065 | dQMultiply2 (qrel,qq,joint->qrel1); |
---|
2066 | |
---|
2067 | return getHingeAngleFromRelativeQuat(qrel, joint->axis1); |
---|
2068 | } |
---|
2069 | return 0; |
---|
2070 | } |
---|
2071 | |
---|
2072 | |
---|
2073 | static dReal getUniversalAngle2(dxJointUniversal *joint) |
---|
2074 | { |
---|
2075 | if (joint->node[0].body) { |
---|
2076 | // length 1 joint axis in global coordinates, from each body |
---|
2077 | dVector3 ax1, ax2; |
---|
2078 | dMatrix3 R; |
---|
2079 | dQuaternion qcross, qq, qrel; |
---|
2080 | |
---|
2081 | getUniversalAxes (joint,ax1,ax2); |
---|
2082 | |
---|
2083 | // It should be possible to get both angles without explicitly |
---|
2084 | // constructing the rotation matrix of the cross. Basically, |
---|
2085 | // orientation of the cross about axis1 comes from body 2, |
---|
2086 | // about axis 2 comes from body 1, and the perpendicular |
---|
2087 | // axis can come from the two bodies somehow. (We don't really |
---|
2088 | // want to assume it's 90 degrees, because in general the |
---|
2089 | // constraints won't be perfectly satisfied, or even very well |
---|
2090 | // satisfied.) |
---|
2091 | // |
---|
2092 | // However, we'd need a version of getHingeAngleFromRElativeQuat() |
---|
2093 | // that CAN handle when its relative quat is rotated along a direction |
---|
2094 | // other than the given axis. What I have here works, |
---|
2095 | // although it's probably much slower than need be. |
---|
2096 | |
---|
2097 | dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
---|
2098 | dRtoQ(R, qcross); |
---|
2099 | |
---|
2100 | if (joint->node[1].body) { |
---|
2101 | dQMultiply1 (qq, joint->node[1].body->q, qcross); |
---|
2102 | dQMultiply2 (qrel,qq,joint->qrel2); |
---|
2103 | } |
---|
2104 | else { |
---|
2105 | // pretend joint->node[1].body->q is the identity |
---|
2106 | dQMultiply2 (qrel,qcross, joint->qrel2); |
---|
2107 | } |
---|
2108 | |
---|
2109 | return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); |
---|
2110 | } |
---|
2111 | return 0; |
---|
2112 | } |
---|
2113 | |
---|
2114 | |
---|
2115 | static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) |
---|
2116 | { |
---|
2117 | info->nub = 4; |
---|
2118 | info->m = 4; |
---|
2119 | |
---|
2120 | // see if we're powered or at a joint limit. |
---|
2121 | bool constraint1 = j->limot1.fmax > 0; |
---|
2122 | bool constraint2 = j->limot2.fmax > 0; |
---|
2123 | |
---|
2124 | bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && |
---|
2125 | j->limot1.lostop <= j->limot1.histop; |
---|
2126 | bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && |
---|
2127 | j->limot2.lostop <= j->limot2.histop; |
---|
2128 | |
---|
2129 | // We need to call testRotationLimit() even if we're motored, since it |
---|
2130 | // records the result. |
---|
2131 | if (limiting1 || limiting2) { |
---|
2132 | dReal angle1, angle2; |
---|
2133 | getUniversalAngles (j, &angle1, &angle2); |
---|
2134 | if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; |
---|
2135 | if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; |
---|
2136 | } |
---|
2137 | if (constraint1) |
---|
2138 | info->m++; |
---|
2139 | if (constraint2) |
---|
2140 | info->m++; |
---|
2141 | } |
---|
2142 | |
---|
2143 | |
---|
2144 | static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) |
---|
2145 | { |
---|
2146 | // set the three ball-and-socket rows |
---|
2147 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
2148 | |
---|
2149 | // set the universal joint row. the angular velocity about an axis |
---|
2150 | // perpendicular to both joint axes should be equal. thus the constraint |
---|
2151 | // equation is |
---|
2152 | // p*w1 - p*w2 = 0 |
---|
2153 | // where p is a vector normal to both joint axes, and w1 and w2 |
---|
2154 | // are the angular velocity vectors of the two bodies. |
---|
2155 | |
---|
2156 | // length 1 joint axis in global coordinates, from each body |
---|
2157 | dVector3 ax1, ax2; |
---|
2158 | dVector3 ax2_temp; |
---|
2159 | // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate |
---|
2160 | // about this. |
---|
2161 | dVector3 p; |
---|
2162 | dReal k; |
---|
2163 | |
---|
2164 | getUniversalAxes(joint, ax1, ax2); |
---|
2165 | k = dDOT(ax1, ax2); |
---|
2166 | ax2_temp[0] = ax2[0] - k*ax1[0]; |
---|
2167 | ax2_temp[1] = ax2[1] - k*ax1[1]; |
---|
2168 | ax2_temp[2] = ax2[2] - k*ax1[2]; |
---|
2169 | dCROSS(p, =, ax1, ax2_temp); |
---|
2170 | dNormalize3(p); |
---|
2171 | |
---|
2172 | int s3=3*info->rowskip; |
---|
2173 | |
---|
2174 | info->J1a[s3+0] = p[0]; |
---|
2175 | info->J1a[s3+1] = p[1]; |
---|
2176 | info->J1a[s3+2] = p[2]; |
---|
2177 | |
---|
2178 | if (joint->node[1].body) { |
---|
2179 | info->J2a[s3+0] = -p[0]; |
---|
2180 | info->J2a[s3+1] = -p[1]; |
---|
2181 | info->J2a[s3+2] = -p[2]; |
---|
2182 | } |
---|
2183 | |
---|
2184 | // compute the right hand side of the constraint equation. set relative |
---|
2185 | // body velocities along p to bring the axes back to perpendicular. |
---|
2186 | // If ax1, ax2 are unit length joint axes as computed from body1 and |
---|
2187 | // body2, we need to rotate both bodies along the axis p. If theta |
---|
2188 | // is the angle between ax1 and ax2, we need an angular velocity |
---|
2189 | // along p to cover the angle erp * (theta - Pi/2) in one step: |
---|
2190 | // |
---|
2191 | // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize |
---|
2192 | // = (erp*fps) * (theta - Pi/2) |
---|
2193 | // |
---|
2194 | // if theta is close to Pi/2, |
---|
2195 | // theta - Pi/2 ~= cos(theta), so |
---|
2196 | // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) |
---|
2197 | |
---|
2198 | info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); |
---|
2199 | |
---|
2200 | // if the first angle is powered, or has joint limits, add in the stuff |
---|
2201 | int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
---|
2202 | |
---|
2203 | // if the second angle is powered, or has joint limits, add in more stuff |
---|
2204 | joint->limot2.addLimot (joint,info,row,ax2,1); |
---|
2205 | } |
---|
2206 | |
---|
2207 | |
---|
2208 | static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) |
---|
2209 | { |
---|
2210 | if (joint->node[0].body) { |
---|
2211 | dVector3 ax1, ax2; |
---|
2212 | dMatrix3 R; |
---|
2213 | dQuaternion qcross; |
---|
2214 | |
---|
2215 | getUniversalAxes(joint, ax1, ax2); |
---|
2216 | |
---|
2217 | // Axis 1. |
---|
2218 | dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
---|
2219 | dRtoQ(R, qcross); |
---|
2220 | dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); |
---|
2221 | |
---|
2222 | // Axis 2. |
---|
2223 | dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
---|
2224 | dRtoQ(R, qcross); |
---|
2225 | if (joint->node[1].body) { |
---|
2226 | dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); |
---|
2227 | } |
---|
2228 | else { |
---|
2229 | // set joint->qrel to qcross |
---|
2230 | for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; |
---|
2231 | } |
---|
2232 | } |
---|
2233 | } |
---|
2234 | |
---|
2235 | |
---|
2236 | void dJointSetUniversalAnchor (dJointID j, dReal x, dReal y, dReal z) |
---|
2237 | { |
---|
2238 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2239 | dUASSERT(joint,"bad joint argument"); |
---|
2240 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2241 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
2242 | universalComputeInitialRelativeRotations(joint); |
---|
2243 | } |
---|
2244 | |
---|
2245 | |
---|
2246 | void dJointSetUniversalAxis1 (dJointID j, dReal x, dReal y, dReal z) |
---|
2247 | { |
---|
2248 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2249 | dUASSERT(joint,"bad joint argument"); |
---|
2250 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2251 | if (joint->flags & dJOINT_REVERSE) |
---|
2252 | setAxes (joint,x,y,z,NULL,joint->axis2); |
---|
2253 | else |
---|
2254 | setAxes (joint,x,y,z,joint->axis1,NULL); |
---|
2255 | universalComputeInitialRelativeRotations(joint); |
---|
2256 | } |
---|
2257 | |
---|
2258 | |
---|
2259 | void dJointSetUniversalAxis2 (dJointID j, dReal x, dReal y, dReal z) |
---|
2260 | { |
---|
2261 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2262 | dUASSERT(joint,"bad joint argument"); |
---|
2263 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2264 | if (joint->flags & dJOINT_REVERSE) |
---|
2265 | setAxes (joint,x,y,z,joint->axis1,NULL); |
---|
2266 | else |
---|
2267 | setAxes (joint,x,y,z,NULL,joint->axis2); |
---|
2268 | universalComputeInitialRelativeRotations(joint); |
---|
2269 | } |
---|
2270 | |
---|
2271 | |
---|
2272 | void dJointGetUniversalAnchor (dJointID j, dVector3 result) |
---|
2273 | { |
---|
2274 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2275 | dUASSERT(joint,"bad joint argument"); |
---|
2276 | dUASSERT(result,"bad result argument"); |
---|
2277 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2278 | if (joint->flags & dJOINT_REVERSE) |
---|
2279 | getAnchor2 (joint,result,joint->anchor2); |
---|
2280 | else |
---|
2281 | getAnchor (joint,result,joint->anchor1); |
---|
2282 | } |
---|
2283 | |
---|
2284 | |
---|
2285 | void dJointGetUniversalAnchor2 (dJointID j, dVector3 result) |
---|
2286 | { |
---|
2287 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2288 | dUASSERT(joint,"bad joint argument"); |
---|
2289 | dUASSERT(result,"bad result argument"); |
---|
2290 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2291 | if (joint->flags & dJOINT_REVERSE) |
---|
2292 | getAnchor (joint,result,joint->anchor1); |
---|
2293 | else |
---|
2294 | getAnchor2 (joint,result,joint->anchor2); |
---|
2295 | } |
---|
2296 | |
---|
2297 | |
---|
2298 | void dJointGetUniversalAxis1 (dJointID j, dVector3 result) |
---|
2299 | { |
---|
2300 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2301 | dUASSERT(joint,"bad joint argument"); |
---|
2302 | dUASSERT(result,"bad result argument"); |
---|
2303 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2304 | if (joint->flags & dJOINT_REVERSE) |
---|
2305 | getAxis2 (joint,result,joint->axis2); |
---|
2306 | else |
---|
2307 | getAxis (joint,result,joint->axis1); |
---|
2308 | } |
---|
2309 | |
---|
2310 | |
---|
2311 | void dJointGetUniversalAxis2 (dJointID j, dVector3 result) |
---|
2312 | { |
---|
2313 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2314 | dUASSERT(joint,"bad joint argument"); |
---|
2315 | dUASSERT(result,"bad result argument"); |
---|
2316 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2317 | if (joint->flags & dJOINT_REVERSE) |
---|
2318 | getAxis (joint,result,joint->axis1); |
---|
2319 | else |
---|
2320 | getAxis2 (joint,result,joint->axis2); |
---|
2321 | } |
---|
2322 | |
---|
2323 | |
---|
2324 | void dJointSetUniversalParam (dJointID j, int parameter, dReal value) |
---|
2325 | { |
---|
2326 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2327 | dUASSERT(joint,"bad joint argument"); |
---|
2328 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2329 | if ((parameter & 0xff00) == 0x100) { |
---|
2330 | joint->limot2.set (parameter & 0xff,value); |
---|
2331 | } |
---|
2332 | else { |
---|
2333 | joint->limot1.set (parameter,value); |
---|
2334 | } |
---|
2335 | } |
---|
2336 | |
---|
2337 | |
---|
2338 | dReal dJointGetUniversalParam (dJointID j, int parameter) |
---|
2339 | { |
---|
2340 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2341 | dUASSERT(joint,"bad joint argument"); |
---|
2342 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2343 | if ((parameter & 0xff00) == 0x100) { |
---|
2344 | return joint->limot2.get (parameter & 0xff); |
---|
2345 | } |
---|
2346 | else { |
---|
2347 | return joint->limot1.get (parameter); |
---|
2348 | } |
---|
2349 | } |
---|
2350 | |
---|
2351 | void dJointGetUniversalAngles (dJointID j, dReal *angle1, dReal *angle2) |
---|
2352 | { |
---|
2353 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2354 | dUASSERT(joint,"bad joint argument"); |
---|
2355 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2356 | if (joint->flags & dJOINT_REVERSE) |
---|
2357 | return getUniversalAngles (joint, angle2, angle1); |
---|
2358 | else |
---|
2359 | return getUniversalAngles (joint, angle1, angle2); |
---|
2360 | } |
---|
2361 | |
---|
2362 | |
---|
2363 | dReal dJointGetUniversalAngle1 (dJointID j) |
---|
2364 | { |
---|
2365 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2366 | dUASSERT(joint,"bad joint argument"); |
---|
2367 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2368 | if (joint->flags & dJOINT_REVERSE) |
---|
2369 | return getUniversalAngle2 (joint); |
---|
2370 | else |
---|
2371 | return getUniversalAngle1 (joint); |
---|
2372 | } |
---|
2373 | |
---|
2374 | |
---|
2375 | dReal dJointGetUniversalAngle2 (dJointID j) |
---|
2376 | { |
---|
2377 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2378 | dUASSERT(joint,"bad joint argument"); |
---|
2379 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2380 | if (joint->flags & dJOINT_REVERSE) |
---|
2381 | return getUniversalAngle1 (joint); |
---|
2382 | else |
---|
2383 | return getUniversalAngle2 (joint); |
---|
2384 | } |
---|
2385 | |
---|
2386 | |
---|
2387 | dReal dJointGetUniversalAngle1Rate (dJointID j) |
---|
2388 | { |
---|
2389 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2390 | dUASSERT(joint,"bad joint argument"); |
---|
2391 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2392 | |
---|
2393 | if (joint->node[0].body) { |
---|
2394 | dVector3 axis; |
---|
2395 | |
---|
2396 | if (joint->flags & dJOINT_REVERSE) |
---|
2397 | getAxis2 (joint,axis,joint->axis2); |
---|
2398 | else |
---|
2399 | getAxis (joint,axis,joint->axis1); |
---|
2400 | |
---|
2401 | dReal rate = dDOT(axis, joint->node[0].body->avel); |
---|
2402 | if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
---|
2403 | return rate; |
---|
2404 | } |
---|
2405 | return 0; |
---|
2406 | } |
---|
2407 | |
---|
2408 | |
---|
2409 | dReal dJointGetUniversalAngle2Rate (dJointID j) |
---|
2410 | { |
---|
2411 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2412 | dUASSERT(joint,"bad joint argument"); |
---|
2413 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2414 | |
---|
2415 | if (joint->node[0].body) { |
---|
2416 | dVector3 axis; |
---|
2417 | |
---|
2418 | if (joint->flags & dJOINT_REVERSE) |
---|
2419 | getAxis (joint,axis,joint->axis1); |
---|
2420 | else |
---|
2421 | getAxis2 (joint,axis,joint->axis2); |
---|
2422 | |
---|
2423 | dReal rate = dDOT(axis, joint->node[0].body->avel); |
---|
2424 | if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
---|
2425 | return rate; |
---|
2426 | } |
---|
2427 | return 0; |
---|
2428 | } |
---|
2429 | |
---|
2430 | |
---|
2431 | void dJointAddUniversalTorques (dJointID j, dReal torque1, dReal torque2) |
---|
2432 | { |
---|
2433 | dxJointUniversal* joint = (dxJointUniversal*)j; |
---|
2434 | dVector3 axis1, axis2; |
---|
2435 | dAASSERT(joint); |
---|
2436 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2437 | |
---|
2438 | if (joint->flags & dJOINT_REVERSE) { |
---|
2439 | dReal temp = torque1; |
---|
2440 | torque1 = - torque2; |
---|
2441 | torque2 = - temp; |
---|
2442 | } |
---|
2443 | |
---|
2444 | getAxis (joint,axis1,joint->axis1); |
---|
2445 | getAxis2 (joint,axis2,joint->axis2); |
---|
2446 | axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; |
---|
2447 | axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; |
---|
2448 | axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; |
---|
2449 | |
---|
2450 | if (joint->node[0].body != 0) |
---|
2451 | dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
---|
2452 | if (joint->node[1].body != 0) |
---|
2453 | dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
---|
2454 | } |
---|
2455 | |
---|
2456 | |
---|
2457 | |
---|
2458 | |
---|
2459 | |
---|
2460 | dxJoint::Vtable __duniversal_vtable = { |
---|
2461 | sizeof(dxJointUniversal), |
---|
2462 | (dxJoint::init_fn*) universalInit, |
---|
2463 | (dxJoint::getInfo1_fn*) universalGetInfo1, |
---|
2464 | (dxJoint::getInfo2_fn*) universalGetInfo2, |
---|
2465 | dJointTypeUniversal}; |
---|
2466 | |
---|
2467 | |
---|
2468 | |
---|
2469 | //**************************************************************************** |
---|
2470 | // Prismatic and Rotoide |
---|
2471 | |
---|
2472 | static void PRInit (dxJointPR *j) |
---|
2473 | { |
---|
2474 | // Default Position |
---|
2475 | // Z^ |
---|
2476 | // | Body 1 P R Body2 |
---|
2477 | // |+---------+ _ _ +-----------+ |
---|
2478 | // || |----|----(_)--------+ | |
---|
2479 | // |+---------+ - +-----------+ |
---|
2480 | // | |
---|
2481 | // X.-----------------------------------------> Y |
---|
2482 | // N.B. X is comming out of the page |
---|
2483 | dSetZero (j->anchor2,4); |
---|
2484 | |
---|
2485 | dSetZero (j->axisR1,4); |
---|
2486 | j->axisR1[0] = 1; |
---|
2487 | dSetZero (j->axisR2,4); |
---|
2488 | j->axisR2[0] = 1; |
---|
2489 | |
---|
2490 | dSetZero (j->axisP1,4); |
---|
2491 | j->axisP1[1] = 1; |
---|
2492 | dSetZero (j->qrel,4); |
---|
2493 | dSetZero (j->offset,4); |
---|
2494 | |
---|
2495 | j->limotR.init (j->world); |
---|
2496 | j->limotP.init (j->world); |
---|
2497 | } |
---|
2498 | |
---|
2499 | |
---|
2500 | dReal dJointGetPRPosition (dJointID j) |
---|
2501 | { |
---|
2502 | dxJointPR* joint = (dxJointPR*)j; |
---|
2503 | dUASSERT(joint,"bad joint argument"); |
---|
2504 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
2505 | |
---|
2506 | dVector3 q; |
---|
2507 | // get the offset in global coordinates |
---|
2508 | dMULTIPLY0_331 (q,joint->node[0].body->posr.R,joint->offset); |
---|
2509 | |
---|
2510 | if (joint->node[1].body) { |
---|
2511 | dVector3 anchor2; |
---|
2512 | |
---|
2513 | // get the anchor2 in global coordinates |
---|
2514 | dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); |
---|
2515 | |
---|
2516 | q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - |
---|
2517 | (joint->node[1].body->posr.pos[0] + anchor2[0]) ); |
---|
2518 | q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - |
---|
2519 | (joint->node[1].body->posr.pos[1] + anchor2[1]) ); |
---|
2520 | q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - |
---|
2521 | (joint->node[1].body->posr.pos[2] + anchor2[2]) ); |
---|
2522 | |
---|
2523 | } |
---|
2524 | else { |
---|
2525 | //N.B. When there is no body 2 the joint->anchor2 is already in |
---|
2526 | // global coordinates |
---|
2527 | |
---|
2528 | q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - |
---|
2529 | (joint->anchor2[0]) ); |
---|
2530 | q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - |
---|
2531 | (joint->anchor2[1]) ); |
---|
2532 | q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - |
---|
2533 | (joint->anchor2[2]) ); |
---|
2534 | |
---|
2535 | } |
---|
2536 | |
---|
2537 | dVector3 axP; |
---|
2538 | // get prismatic axis in global coordinates |
---|
2539 | dMULTIPLY0_331 (axP,joint->node[0].body->posr.R,joint->axisP1); |
---|
2540 | |
---|
2541 | return dDOT(axP, q); |
---|
2542 | } |
---|
2543 | |
---|
2544 | |
---|
2545 | dReal dJointGetPRPositionRate (dJointID j) |
---|
2546 | { |
---|
2547 | dxJointPR* joint = (dxJointPR*)j; |
---|
2548 | dUASSERT(joint,"bad joint argument"); |
---|
2549 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
2550 | |
---|
2551 | if (joint->node[0].body) { |
---|
2552 | // We want to find the rate of change of the prismatic part of the joint |
---|
2553 | // We can find it by looking at the speed difference between body1 and the |
---|
2554 | // anchor point. |
---|
2555 | |
---|
2556 | // r will be used to find the distance between body1 and the anchor point |
---|
2557 | dVector3 r; |
---|
2558 | if (joint->node[1].body) { |
---|
2559 | // Find joint->anchor2 in global coordinates |
---|
2560 | dVector3 anchor2; |
---|
2561 | dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); |
---|
2562 | |
---|
2563 | r[0] = joint->node[0].body->posr.pos[0] - anchor2[0]; |
---|
2564 | r[1] = joint->node[0].body->posr.pos[1] - anchor2[1]; |
---|
2565 | r[2] = joint->node[0].body->posr.pos[2] - anchor2[2]; |
---|
2566 | } |
---|
2567 | else { |
---|
2568 | //N.B. When there is no body 2 the joint->anchor2 is already in |
---|
2569 | // global coordinates |
---|
2570 | r[0] = joint->node[0].body->posr.pos[0] - joint->anchor2[0]; |
---|
2571 | r[1] = joint->node[0].body->posr.pos[1] - joint->anchor2[1]; |
---|
2572 | r[2] = joint->node[0].body->posr.pos[2] - joint->anchor2[2]; |
---|
2573 | } |
---|
2574 | |
---|
2575 | // The body1 can have velocity coming from the rotation of |
---|
2576 | // the rotoide axis. We need to remove this. |
---|
2577 | |
---|
2578 | // Take only the angular rotation coming from the rotation |
---|
2579 | // of the rotoide articulation |
---|
2580 | // N.B. Body1 and Body2 should have the same rotation along axis |
---|
2581 | // other than the rotoide axis. |
---|
2582 | dVector3 angular; |
---|
2583 | dMULTIPLY0_331 (angular,joint->node[0].body->posr.R,joint->axisR1); |
---|
2584 | dReal omega = dDOT(angular, joint->node[0].body->avel); |
---|
2585 | angular[0] *= omega; |
---|
2586 | angular[1] *= omega; |
---|
2587 | angular[2] *= omega; |
---|
2588 | |
---|
2589 | // Find the contribution of the angular rotation to the linear speed |
---|
2590 | // N.B. We do vel = r X w instead of vel = w x r to have vel negative |
---|
2591 | // since we want to remove it from the linear velocity of the body |
---|
2592 | dVector3 lvel1; |
---|
2593 | dCROSS(lvel1, =, r, angular); |
---|
2594 | |
---|
2595 | lvel1[0] += joint->node[0].body->lvel[0]; |
---|
2596 | lvel1[1] += joint->node[0].body->lvel[1]; |
---|
2597 | lvel1[2] += joint->node[0].body->lvel[2]; |
---|
2598 | |
---|
2599 | // Since we want rate of change along the prismatic axis |
---|
2600 | // get axisP1 in global coordinates and get the component |
---|
2601 | // along this axis only |
---|
2602 | dVector3 axP1; |
---|
2603 | dMULTIPLY0_331 (axP1,joint->node[0].body->posr.R,joint->axisP1); |
---|
2604 | return dDOT(axP1, lvel1); |
---|
2605 | } |
---|
2606 | |
---|
2607 | return 0.0; |
---|
2608 | } |
---|
2609 | |
---|
2610 | |
---|
2611 | |
---|
2612 | static void PRGetInfo1 (dxJointPR *j, dxJoint::Info1 *info) |
---|
2613 | { |
---|
2614 | info->m = 4; |
---|
2615 | info->nub = 4; |
---|
2616 | |
---|
2617 | bool added = false; |
---|
2618 | |
---|
2619 | added = false; |
---|
2620 | // see if the prismatic articulation is powered |
---|
2621 | if (j->limotP.fmax > 0) |
---|
2622 | { |
---|
2623 | added = true; |
---|
2624 | (info->m)++; // powered needs an extra constraint row |
---|
2625 | } |
---|
2626 | |
---|
2627 | // see if we're at a joint limit. |
---|
2628 | j->limotP.limit = 0; |
---|
2629 | if ((j->limotP.lostop > -dInfinity || j->limotP.histop < dInfinity) && |
---|
2630 | j->limotP.lostop <= j->limotP.histop) { |
---|
2631 | // measure joint position |
---|
2632 | dReal pos = dJointGetPRPosition (j); |
---|
2633 | if (pos <= j->limotP.lostop) { |
---|
2634 | j->limotP.limit = 1; |
---|
2635 | j->limotP.limit_err = pos - j->limotP.lostop; |
---|
2636 | if (!added) |
---|
2637 | (info->m)++; |
---|
2638 | } |
---|
2639 | |
---|
2640 | if (pos >= j->limotP.histop) { |
---|
2641 | j->limotP.limit = 2; |
---|
2642 | j->limotP.limit_err = pos - j->limotP.histop; |
---|
2643 | if (!added) |
---|
2644 | (info->m)++; |
---|
2645 | } |
---|
2646 | } |
---|
2647 | |
---|
2648 | } |
---|
2649 | |
---|
2650 | |
---|
2651 | |
---|
2652 | static void PRGetInfo2 (dxJointPR *joint, dxJoint::Info2 *info) |
---|
2653 | { |
---|
2654 | int s = info->rowskip; |
---|
2655 | int s2= 2*s; |
---|
2656 | int s3= 3*s; |
---|
2657 | int s4= 4*s; |
---|
2658 | |
---|
2659 | dReal k = info->fps * info->erp; |
---|
2660 | |
---|
2661 | |
---|
2662 | dVector3 q; // plane space of axP and after that axR |
---|
2663 | |
---|
2664 | // pull out pos and R for both bodies. also get the `connection' |
---|
2665 | // vector pos2-pos1. |
---|
2666 | |
---|
2667 | dReal *pos1,*pos2,*R1,*R2; |
---|
2668 | pos1 = joint->node[0].body->posr.pos; |
---|
2669 | R1 = joint->node[0].body->posr.R; |
---|
2670 | if (joint->node[1].body) { |
---|
2671 | pos2 = joint->node[1].body->posr.pos; |
---|
2672 | R2 = joint->node[1].body->posr.R; |
---|
2673 | } |
---|
2674 | else { |
---|
2675 | // pos2 = 0; // N.B. We can do that to be safe but it is no necessary |
---|
2676 | // R2 = 0; // N.B. We can do that to be safe but it is no necessary |
---|
2677 | } |
---|
2678 | |
---|
2679 | |
---|
2680 | dVector3 axP; // Axis of the prismatic joint in global frame |
---|
2681 | dMULTIPLY0_331 (axP, R1, joint->axisP1); |
---|
2682 | |
---|
2683 | // distance between the body1 and the anchor2 in global frame |
---|
2684 | // Calculated in the same way as the offset |
---|
2685 | dVector3 dist; |
---|
2686 | |
---|
2687 | if (joint->node[1].body) |
---|
2688 | { |
---|
2689 | dMULTIPLY0_331 (dist, R2, joint->anchor2); |
---|
2690 | dist[0] += pos2[0] - pos1[0]; |
---|
2691 | dist[1] += pos2[1] - pos1[1]; |
---|
2692 | dist[2] += pos2[2] - pos1[2]; |
---|
2693 | } |
---|
2694 | else { |
---|
2695 | dist[0] = joint->anchor2[0] - pos1[0]; |
---|
2696 | dist[1] = joint->anchor2[1] - pos1[1]; |
---|
2697 | dist[2] = joint->anchor2[2] - pos1[2]; |
---|
2698 | } |
---|
2699 | |
---|
2700 | |
---|
2701 | // ====================================================================== |
---|
2702 | // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered |
---|
2703 | |
---|
2704 | // Set the two rotoide rows. The rotoide axis should be the only unconstrained |
---|
2705 | // rotational axis, the angular velocity of the two bodies perpendicular to |
---|
2706 | // the rotoide axis should be equal. Thus the constraint equations are |
---|
2707 | // p*w1 - p*w2 = 0 |
---|
2708 | // q*w1 - q*w2 = 0 |
---|
2709 | // where p and q are unit vectors normal to the rotoide axis, and w1 and w2 |
---|
2710 | // are the angular velocity vectors of the two bodies. |
---|
2711 | dVector3 ax1; |
---|
2712 | dMULTIPLY0_331 (ax1, joint->node[0].body->posr.R, joint->axisR1); |
---|
2713 | dCROSS(q , =, ax1, axP); |
---|
2714 | |
---|
2715 | info->J1a[0] = axP[0]; |
---|
2716 | info->J1a[1] = axP[1]; |
---|
2717 | info->J1a[2] = axP[2]; |
---|
2718 | info->J1a[s+0] = q[0]; |
---|
2719 | info->J1a[s+1] = q[1]; |
---|
2720 | info->J1a[s+2] = q[2]; |
---|
2721 | |
---|
2722 | if (joint->node[1].body) { |
---|
2723 | info->J2a[0] = -axP[0]; |
---|
2724 | info->J2a[1] = -axP[1]; |
---|
2725 | info->J2a[2] = -axP[2]; |
---|
2726 | info->J2a[s+0] = -q[0]; |
---|
2727 | info->J2a[s+1] = -q[1]; |
---|
2728 | info->J2a[s+2] = -q[2]; |
---|
2729 | } |
---|
2730 | |
---|
2731 | |
---|
2732 | // Compute the right hand side of the constraint equation set. Relative |
---|
2733 | // body velocities along p and q to bring the rotoide back into alignment. |
---|
2734 | // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame. |
---|
2735 | // We need to rotate both bodies along the axis u = (ax1 x ax2). |
---|
2736 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
---|
2737 | // along u to cover angle erp*theta in one step : |
---|
2738 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
2739 | // = (erp*fps) * theta |
---|
2740 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
---|
2741 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
---|
2742 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
---|
2743 | // theta ~= sin(theta), so |
---|
2744 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
---|
2745 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
---|
2746 | // velocity to p and q to find the right hand side. |
---|
2747 | |
---|
2748 | dVector3 ax2; |
---|
2749 | if (joint->node[1].body) { |
---|
2750 | dMULTIPLY0_331 (ax2, R2, joint->axisR2); |
---|
2751 | } |
---|
2752 | else { |
---|
2753 | ax2[0] = joint->axisR2[0]; |
---|
2754 | ax2[1] = joint->axisR2[1]; |
---|
2755 | ax2[2] = joint->axisR2[2]; |
---|
2756 | } |
---|
2757 | |
---|
2758 | dVector3 b; |
---|
2759 | dCROSS (b,=,ax1, ax2); |
---|
2760 | info->c[0] = k * dDOT(b, axP); |
---|
2761 | info->c[1] = k * dDOT(b, q); |
---|
2762 | |
---|
2763 | |
---|
2764 | |
---|
2765 | // ========================== |
---|
2766 | // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered |
---|
2767 | // or 5 if rotoide and prismatic powered |
---|
2768 | |
---|
2769 | // two rows. we want: vel2 = vel1 + w1 x c ... but this would |
---|
2770 | // result in three equations, so we project along the planespace vectors |
---|
2771 | // so that sliding along the prismatic axis is disregarded. for symmetry we |
---|
2772 | // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. |
---|
2773 | |
---|
2774 | // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist' |
---|
2775 | // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p |
---|
2776 | // v_p is speed of prismatic joint (i.e. elongation rate) |
---|
2777 | // Since the constraints are perpendicular to v_p we have: |
---|
2778 | // p dot v_p = 0 and q dot v_p = 0 |
---|
2779 | // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) |
---|
2780 | // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) |
---|
2781 | // == |
---|
2782 | // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist |
---|
2783 | // since a . (b x c) = - b . (a x c) = - (a x c) . b |
---|
2784 | // and a x b = - b x a |
---|
2785 | // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0 |
---|
2786 | // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0 |
---|
2787 | // Coeff for 1er line of: J1l => ax1, J2l => -ax1 |
---|
2788 | // Coeff for 2er line of: J1l => q, J2l => -q |
---|
2789 | // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1 |
---|
2790 | // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q |
---|
2791 | |
---|
2792 | |
---|
2793 | dCROSS ((info->J1a)+s2, = , dist, ax1); |
---|
2794 | |
---|
2795 | dCROSS ((info->J1a)+s3, = , dist, q); |
---|
2796 | |
---|
2797 | |
---|
2798 | info->J1l[s2+0] = ax1[0]; |
---|
2799 | info->J1l[s2+1] = ax1[1]; |
---|
2800 | info->J1l[s2+2] = ax1[2]; |
---|
2801 | |
---|
2802 | info->J1l[s3+0] = q[0]; |
---|
2803 | info->J1l[s3+1] = q[1]; |
---|
2804 | info->J1l[s3+2] = q[2]; |
---|
2805 | |
---|
2806 | if (joint->node[1].body) { |
---|
2807 | dVector3 anchor2; |
---|
2808 | |
---|
2809 | // Calculate anchor2 in world coordinate |
---|
2810 | dMULTIPLY0_331 (anchor2, R2, joint->anchor2); |
---|
2811 | |
---|
2812 | // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value |
---|
2813 | dCROSS ((info->J2a)+s2, = , ax2, anchor2); // since ax1 == ax2 |
---|
2814 | |
---|
2815 | // The cross product is in reverse order since we want the negative value |
---|
2816 | dCROSS ((info->J2a)+s3, = , q, anchor2); |
---|
2817 | |
---|
2818 | info->J2l[s2+0] = -ax1[0]; |
---|
2819 | info->J2l[s2+1] = -ax1[1]; |
---|
2820 | info->J2l[s2+2] = -ax1[2]; |
---|
2821 | |
---|
2822 | info->J2l[s3+0] = -q[0]; |
---|
2823 | info->J2l[s3+1] = -q[1]; |
---|
2824 | info->J2l[s3+2] = -q[2]; |
---|
2825 | } |
---|
2826 | |
---|
2827 | |
---|
2828 | // We want to make correction for motion not in the line of the axisP |
---|
2829 | // We calculate the displacement w.r.t. the anchor pt. |
---|
2830 | // |
---|
2831 | // compute the elements 2 and 3 of right hand side. |
---|
2832 | // we want to align the offset point (in body 2's frame) with the center of body 1. |
---|
2833 | // The position should be the same when we are not along the prismatic axis |
---|
2834 | dVector3 err; |
---|
2835 | dMULTIPLY0_331 (err, R1, joint->offset); |
---|
2836 | err[0] += dist[0]; |
---|
2837 | err[1] += dist[1]; |
---|
2838 | err[2] += dist[2]; |
---|
2839 | info->c[2] = k * dDOT(ax1, err); |
---|
2840 | info->c[3] = k * dDOT(q, err); |
---|
2841 | |
---|
2842 | // Here we can't use addLimot because of some assumption in the function |
---|
2843 | int powered = joint->limotP.fmax > 0; |
---|
2844 | if (powered || joint->limotP.limit) { |
---|
2845 | info->J1l[s4+0] = axP[0]; |
---|
2846 | info->J1l[s4+1] = axP[1]; |
---|
2847 | info->J1l[s4+2] = axP[2]; |
---|
2848 | if (joint->node[1].body) { |
---|
2849 | info->J2l[s4+0] = -axP[0]; |
---|
2850 | info->J2l[s4+1] = -axP[1]; |
---|
2851 | info->J2l[s4+2] = -axP[2]; |
---|
2852 | } |
---|
2853 | // linear limot torque decoupling step: |
---|
2854 | // |
---|
2855 | // if this is a linear limot (e.g. from a slider), we have to be careful |
---|
2856 | // that the linear constraint forces (+/- ax1) applied to the two bodies |
---|
2857 | // do not create a torque couple. in other words, the points that the |
---|
2858 | // constraint force is applied at must lie along the same ax1 axis. |
---|
2859 | // a torque couple will result in powered or limited slider-jointed free |
---|
2860 | // bodies from gaining angular momentum. |
---|
2861 | // the solution used here is to apply the constraint forces at the point |
---|
2862 | // halfway between the body centers. there is no penalty (other than an |
---|
2863 | // extra tiny bit of computation) in doing this adjustment. note that we |
---|
2864 | // only need to do this if the constraint connects two bodies. |
---|
2865 | |
---|
2866 | dVector3 ltd; // Linear Torque Decoupling vector (a torque) |
---|
2867 | if (joint->node[1].body) { |
---|
2868 | dVector3 c; |
---|
2869 | c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); |
---|
2870 | c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); |
---|
2871 | c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); |
---|
2872 | dReal val = dDOT(q, c); |
---|
2873 | c[0] -= val * c[0]; |
---|
2874 | c[1] -= val * c[1]; |
---|
2875 | c[2] -= val * c[2]; |
---|
2876 | |
---|
2877 | dCROSS (ltd,=,c,axP); |
---|
2878 | info->J1a[s4+0] = ltd[0]; |
---|
2879 | info->J1a[s4+1] = ltd[1]; |
---|
2880 | info->J1a[s4+2] = ltd[2]; |
---|
2881 | info->J2a[s4+0] = ltd[0]; |
---|
2882 | info->J2a[s4+1] = ltd[1]; |
---|
2883 | info->J2a[s4+2] = ltd[2]; |
---|
2884 | } |
---|
2885 | |
---|
2886 | // if we're limited low and high simultaneously, the joint motor is |
---|
2887 | // ineffective |
---|
2888 | if (joint->limotP.limit && (joint->limotP.lostop == joint->limotP.histop)) |
---|
2889 | powered = 0; |
---|
2890 | |
---|
2891 | int row = 4; |
---|
2892 | if (powered) { |
---|
2893 | info->cfm[row] = joint->limotP.normal_cfm; |
---|
2894 | if (!joint->limotP.limit) { |
---|
2895 | info->c[row] = joint->limotP.vel; |
---|
2896 | info->lo[row] = -joint->limotP.fmax; |
---|
2897 | info->hi[row] = joint->limotP.fmax; |
---|
2898 | } |
---|
2899 | else { |
---|
2900 | // the joint is at a limit, AND is being powered. if the joint is |
---|
2901 | // being powered into the limit then we apply the maximum motor force |
---|
2902 | // in that direction, because the motor is working against the |
---|
2903 | // immovable limit. if the joint is being powered away from the limit |
---|
2904 | // then we have problems because actually we need *two* lcp |
---|
2905 | // constraints to handle this case. so we fake it and apply some |
---|
2906 | // fraction of the maximum force. the fraction to use can be set as |
---|
2907 | // a fudge factor. |
---|
2908 | |
---|
2909 | dReal fm = joint->limotP.fmax; |
---|
2910 | dReal vel = joint->limotP.vel; |
---|
2911 | int limit = joint->limotP.limit; |
---|
2912 | if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; |
---|
2913 | |
---|
2914 | // if we're powering away from the limit, apply the fudge factor |
---|
2915 | if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) |
---|
2916 | fm *= joint->limotP.fudge_factor; |
---|
2917 | |
---|
2918 | |
---|
2919 | dBodyAddForce (joint->node[0].body,-fm*axP[0],-fm*axP[1],-fm*axP[2]); |
---|
2920 | |
---|
2921 | if (joint->node[1].body) { |
---|
2922 | dBodyAddForce (joint->node[1].body,fm*axP[0],fm*axP[1],fm*axP[2]); |
---|
2923 | |
---|
2924 | // linear limot torque decoupling step: refer to above discussion |
---|
2925 | dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], |
---|
2926 | -fm*ltd[2]); |
---|
2927 | dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], |
---|
2928 | -fm*ltd[2]); |
---|
2929 | } |
---|
2930 | } |
---|
2931 | } |
---|
2932 | |
---|
2933 | if (joint->limotP.limit) { |
---|
2934 | dReal k = info->fps * joint->limotP.stop_erp; |
---|
2935 | info->c[row] = -k * joint->limotP.limit_err; |
---|
2936 | info->cfm[row] = joint->limotP.stop_cfm; |
---|
2937 | |
---|
2938 | if (joint->limotP.lostop == joint->limotP.histop) { |
---|
2939 | // limited low and high simultaneously |
---|
2940 | info->lo[row] = -dInfinity; |
---|
2941 | info->hi[row] = dInfinity; |
---|
2942 | } |
---|
2943 | else { |
---|
2944 | if (joint->limotP.limit == 1) { |
---|
2945 | // low limit |
---|
2946 | info->lo[row] = 0; |
---|
2947 | info->hi[row] = dInfinity; |
---|
2948 | } |
---|
2949 | else { |
---|
2950 | // high limit |
---|
2951 | info->lo[row] = -dInfinity; |
---|
2952 | info->hi[row] = 0; |
---|
2953 | } |
---|
2954 | |
---|
2955 | // deal with bounce |
---|
2956 | if (joint->limotP.bounce > 0) { |
---|
2957 | // calculate joint velocity |
---|
2958 | dReal vel; |
---|
2959 | vel = dDOT(joint->node[0].body->lvel, axP); |
---|
2960 | if (joint->node[1].body) |
---|
2961 | vel -= dDOT(joint->node[1].body->lvel, axP); |
---|
2962 | |
---|
2963 | // only apply bounce if the velocity is incoming, and if the |
---|
2964 | // resulting c[] exceeds what we already have. |
---|
2965 | if (joint->limotP.limit == 1) { |
---|
2966 | // low limit |
---|
2967 | if (vel < 0) { |
---|
2968 | dReal newc = -joint->limotP.bounce * vel; |
---|
2969 | if (newc > info->c[row]) info->c[row] = newc; |
---|
2970 | } |
---|
2971 | } |
---|
2972 | else { |
---|
2973 | // high limit - all those computations are reversed |
---|
2974 | if (vel > 0) { |
---|
2975 | dReal newc = -joint->limotP.bounce * vel; |
---|
2976 | if (newc < info->c[row]) info->c[row] = newc; |
---|
2977 | } |
---|
2978 | } |
---|
2979 | } |
---|
2980 | } |
---|
2981 | } |
---|
2982 | } |
---|
2983 | } |
---|
2984 | |
---|
2985 | |
---|
2986 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
2987 | static void PRComputeInitialRelativeRotation (dxJointPR *joint) |
---|
2988 | { |
---|
2989 | if (joint->node[0].body) { |
---|
2990 | if (joint->node[1].body) { |
---|
2991 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
2992 | } |
---|
2993 | else { |
---|
2994 | // set joint->qrel to the transpose of the first body q |
---|
2995 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
2996 | for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
2997 | // WARNING do we need the - in -joint->node[0].body->q[i]; or not |
---|
2998 | } |
---|
2999 | } |
---|
3000 | } |
---|
3001 | |
---|
3002 | void dJointSetPRAnchor (dJointID j, dReal x, dReal y, dReal z) |
---|
3003 | { |
---|
3004 | dxJointPR* joint = (dxJointPR*)j; |
---|
3005 | dUASSERT(joint,"bad joint argument"); |
---|
3006 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3007 | |
---|
3008 | dVector3 dummy; |
---|
3009 | setAnchors (joint,x,y,z,dummy,joint->anchor2); |
---|
3010 | } |
---|
3011 | |
---|
3012 | |
---|
3013 | void dJointSetPRAxis1 (dJointID j, dReal x, dReal y, dReal z) |
---|
3014 | { |
---|
3015 | dxJointPR* joint = (dxJointPR*)j; |
---|
3016 | dUASSERT(joint,"bad joint argument"); |
---|
3017 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3018 | |
---|
3019 | setAxes (joint,x,y,z,joint->axisP1, 0); |
---|
3020 | |
---|
3021 | PRComputeInitialRelativeRotation (joint); |
---|
3022 | |
---|
3023 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
3024 | // also compute distance between anchor of body1 w.r.t center of body 2 |
---|
3025 | dVector3 c; |
---|
3026 | if (joint->node[1].body) { |
---|
3027 | dVector3 anchor2; |
---|
3028 | dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R, joint->anchor2); |
---|
3029 | |
---|
3030 | c[0] = ( joint->node[1].body->posr.pos[0] + anchor2[0] - |
---|
3031 | joint->node[0].body->posr.pos[0] ); |
---|
3032 | c[1] = ( joint->node[1].body->posr.pos[1] + anchor2[1] - |
---|
3033 | joint->node[0].body->posr.pos[1] ); |
---|
3034 | c[2] = ( joint->node[1].body->posr.pos[2] + anchor2[2] - |
---|
3035 | joint->node[0].body->posr.pos[2] ); |
---|
3036 | } |
---|
3037 | else if (joint->node[0].body) { |
---|
3038 | c[0] = joint->anchor2[0] - joint->node[0].body->posr.pos[0]; |
---|
3039 | c[1] = joint->anchor2[1] - joint->node[0].body->posr.pos[1]; |
---|
3040 | c[2] = joint->anchor2[2] - joint->node[0].body->posr.pos[2]; |
---|
3041 | } |
---|
3042 | else |
---|
3043 | { |
---|
3044 | joint->offset[0] = joint->anchor2[0]; |
---|
3045 | joint->offset[1] = joint->anchor2[1]; |
---|
3046 | joint->offset[2] = joint->anchor2[2]; |
---|
3047 | |
---|
3048 | return; |
---|
3049 | } |
---|
3050 | |
---|
3051 | |
---|
3052 | dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,c); |
---|
3053 | } |
---|
3054 | |
---|
3055 | |
---|
3056 | void dJointSetPRAxis2 (dJointID j, dReal x, dReal y, dReal z) |
---|
3057 | { |
---|
3058 | dxJointPR* joint = (dxJointPR*)j; |
---|
3059 | dUASSERT(joint,"bad joint argument"); |
---|
3060 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3061 | setAxes (joint,x,y,z,joint->axisR1,joint->axisR2); |
---|
3062 | PRComputeInitialRelativeRotation (joint); |
---|
3063 | } |
---|
3064 | |
---|
3065 | |
---|
3066 | void dJointSetPRParam (dJointID j, int parameter, dReal value) |
---|
3067 | { |
---|
3068 | dxJointPR* joint = (dxJointPR*)j; |
---|
3069 | dUASSERT(joint,"bad joint argument"); |
---|
3070 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3071 | if ((parameter & 0xff00) == 0x100) { |
---|
3072 | joint->limotR.set (parameter,value); |
---|
3073 | } |
---|
3074 | else { |
---|
3075 | joint->limotP.set (parameter & 0xff,value); |
---|
3076 | } |
---|
3077 | } |
---|
3078 | |
---|
3079 | void dJointGetPRAnchor (dJointID j, dVector3 result) |
---|
3080 | { |
---|
3081 | dxJointPR* joint = (dxJointPR*)j; |
---|
3082 | dUASSERT(joint,"bad joint argument"); |
---|
3083 | dUASSERT(result,"bad result argument"); |
---|
3084 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3085 | |
---|
3086 | if (joint->node[1].body) |
---|
3087 | getAnchor2 (joint,result,joint->anchor2); |
---|
3088 | else |
---|
3089 | { |
---|
3090 | result[0] = joint->anchor2[0]; |
---|
3091 | result[1] = joint->anchor2[1]; |
---|
3092 | result[2] = joint->anchor2[2]; |
---|
3093 | } |
---|
3094 | |
---|
3095 | } |
---|
3096 | |
---|
3097 | void dJointGetPRAxis1 (dJointID j, dVector3 result) |
---|
3098 | { |
---|
3099 | dxJointPR* joint = (dxJointPR*)j; |
---|
3100 | dUASSERT(joint,"bad joint argument"); |
---|
3101 | dUASSERT(result,"bad result argument"); |
---|
3102 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3103 | getAxis(joint, result, joint->axisP1); |
---|
3104 | } |
---|
3105 | |
---|
3106 | void dJointGetPRAxis2 (dJointID j, dVector3 result) |
---|
3107 | { |
---|
3108 | dxJointPR* joint = (dxJointPR*)j; |
---|
3109 | dUASSERT(joint,"bad joint argument"); |
---|
3110 | dUASSERT(result,"bad result argument"); |
---|
3111 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3112 | getAxis(joint, result, joint->axisR1); |
---|
3113 | } |
---|
3114 | |
---|
3115 | dReal dJointGetPRParam (dJointID j, int parameter) |
---|
3116 | { |
---|
3117 | dxJointPR* joint = (dxJointPR*)j; |
---|
3118 | dUASSERT(joint,"bad joint argument"); |
---|
3119 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not Prismatic and Rotoide"); |
---|
3120 | if ((parameter & 0xff00) == 0x100) { |
---|
3121 | return joint->limotR.get (parameter & 0xff); |
---|
3122 | } |
---|
3123 | else { |
---|
3124 | return joint->limotP.get (parameter); |
---|
3125 | } |
---|
3126 | } |
---|
3127 | |
---|
3128 | void dJointAddPRTorque (dJointID j, dReal torque) |
---|
3129 | { |
---|
3130 | dxJointPR* joint = (dxJointPR*)j; |
---|
3131 | dVector3 axis; |
---|
3132 | dAASSERT(joint); |
---|
3133 | dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); |
---|
3134 | |
---|
3135 | if (joint->flags & dJOINT_REVERSE) |
---|
3136 | torque = -torque; |
---|
3137 | |
---|
3138 | getAxis (joint,axis,joint->axisR1); |
---|
3139 | axis[0] *= torque; |
---|
3140 | axis[1] *= torque; |
---|
3141 | axis[2] *= torque; |
---|
3142 | |
---|
3143 | if (joint->node[0].body != 0) |
---|
3144 | dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); |
---|
3145 | if (joint->node[1].body != 0) |
---|
3146 | dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
---|
3147 | } |
---|
3148 | |
---|
3149 | |
---|
3150 | dxJoint::Vtable __dPR_vtable = { |
---|
3151 | sizeof(dxJointPR), |
---|
3152 | (dxJoint::init_fn*) PRInit, |
---|
3153 | (dxJoint::getInfo1_fn*) PRGetInfo1, |
---|
3154 | (dxJoint::getInfo2_fn*) PRGetInfo2, |
---|
3155 | dJointTypePR |
---|
3156 | }; |
---|
3157 | |
---|
3158 | |
---|
3159 | //**************************************************************************** |
---|
3160 | // angular motor |
---|
3161 | |
---|
3162 | static void amotorInit (dxJointAMotor *j) |
---|
3163 | { |
---|
3164 | int i; |
---|
3165 | j->num = 0; |
---|
3166 | j->mode = dAMotorUser; |
---|
3167 | for (i=0; i<3; i++) { |
---|
3168 | j->rel[i] = 0; |
---|
3169 | dSetZero (j->axis[i],4); |
---|
3170 | j->limot[i].init (j->world); |
---|
3171 | j->angle[i] = 0; |
---|
3172 | } |
---|
3173 | dSetZero (j->reference1,4); |
---|
3174 | dSetZero (j->reference2,4); |
---|
3175 | } |
---|
3176 | |
---|
3177 | |
---|
3178 | // compute the 3 axes in global coordinates |
---|
3179 | |
---|
3180 | static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) |
---|
3181 | { |
---|
3182 | if (joint->mode == dAMotorEuler) { |
---|
3183 | // special handling for euler mode |
---|
3184 | dMULTIPLY0_331 (ax[0],joint->node[0].body->posr.R,joint->axis[0]); |
---|
3185 | if (joint->node[1].body) { |
---|
3186 | dMULTIPLY0_331 (ax[2],joint->node[1].body->posr.R,joint->axis[2]); |
---|
3187 | } |
---|
3188 | else { |
---|
3189 | ax[2][0] = joint->axis[2][0]; |
---|
3190 | ax[2][1] = joint->axis[2][1]; |
---|
3191 | ax[2][2] = joint->axis[2][2]; |
---|
3192 | } |
---|
3193 | dCROSS (ax[1],=,ax[2],ax[0]); |
---|
3194 | dNormalize3 (ax[1]); |
---|
3195 | } |
---|
3196 | else { |
---|
3197 | for (int i=0; i < joint->num; i++) { |
---|
3198 | if (joint->rel[i] == 1) { |
---|
3199 | // relative to b1 |
---|
3200 | dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); |
---|
3201 | } |
---|
3202 | else if (joint->rel[i] == 2) { |
---|
3203 | // relative to b2 |
---|
3204 | if (joint->node[1].body) { // jds: don't assert, just ignore |
---|
3205 | dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); |
---|
3206 | } |
---|
3207 | } |
---|
3208 | else { |
---|
3209 | // global - just copy it |
---|
3210 | ax[i][0] = joint->axis[i][0]; |
---|
3211 | ax[i][1] = joint->axis[i][1]; |
---|
3212 | ax[i][2] = joint->axis[i][2]; |
---|
3213 | } |
---|
3214 | } |
---|
3215 | } |
---|
3216 | } |
---|
3217 | |
---|
3218 | |
---|
3219 | static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) |
---|
3220 | { |
---|
3221 | // assumptions: |
---|
3222 | // global axes already calculated --> ax |
---|
3223 | // axis[0] is relative to body 1 --> global ax[0] |
---|
3224 | // axis[2] is relative to body 2 --> global ax[2] |
---|
3225 | // ax[1] = ax[2] x ax[0] |
---|
3226 | // original ax[0] and ax[2] are perpendicular |
---|
3227 | // reference1 is perpendicular to ax[0] (in body 1 frame) |
---|
3228 | // reference2 is perpendicular to ax[2] (in body 2 frame) |
---|
3229 | // all ax[] and reference vectors are unit length |
---|
3230 | |
---|
3231 | // calculate references in global frame |
---|
3232 | dVector3 ref1,ref2; |
---|
3233 | dMULTIPLY0_331 (ref1,joint->node[0].body->posr.R,joint->reference1); |
---|
3234 | if (joint->node[1].body) { |
---|
3235 | dMULTIPLY0_331 (ref2,joint->node[1].body->posr.R,joint->reference2); |
---|
3236 | } |
---|
3237 | else { |
---|
3238 | ref2[0] = joint->reference2[0]; |
---|
3239 | ref2[1] = joint->reference2[1]; |
---|
3240 | ref2[2] = joint->reference2[2]; |
---|
3241 | } |
---|
3242 | |
---|
3243 | // get q perpendicular to both ax[0] and ref1, get first euler angle |
---|
3244 | dVector3 q; |
---|
3245 | dCROSS (q,=,ax[0],ref1); |
---|
3246 | joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); |
---|
3247 | |
---|
3248 | // get q perpendicular to both ax[0] and ax[1], get second euler angle |
---|
3249 | dCROSS (q,=,ax[0],ax[1]); |
---|
3250 | joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); |
---|
3251 | |
---|
3252 | // get q perpendicular to both ax[1] and ax[2], get third euler angle |
---|
3253 | dCROSS (q,=,ax[1],ax[2]); |
---|
3254 | joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); |
---|
3255 | } |
---|
3256 | |
---|
3257 | |
---|
3258 | // set the reference vectors as follows: |
---|
3259 | // * reference1 = current axis[2] relative to body 1 |
---|
3260 | // * reference2 = current axis[0] relative to body 2 |
---|
3261 | // this assumes that: |
---|
3262 | // * axis[0] is relative to body 1 |
---|
3263 | // * axis[2] is relative to body 2 |
---|
3264 | |
---|
3265 | static void amotorSetEulerReferenceVectors (dxJointAMotor *j) |
---|
3266 | { |
---|
3267 | if (j->node[0].body && j->node[1].body) { |
---|
3268 | dVector3 r; // axis[2] and axis[0] in global coordinates |
---|
3269 | dMULTIPLY0_331 (r,j->node[1].body->posr.R,j->axis[2]); |
---|
3270 | dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); |
---|
3271 | dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); |
---|
3272 | dMULTIPLY1_331 (j->reference2,j->node[1].body->posr.R,r); |
---|
3273 | } |
---|
3274 | |
---|
3275 | else { // jds |
---|
3276 | // else if (j->node[0].body) { |
---|
3277 | // dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]); |
---|
3278 | // dMULTIPLY0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]); |
---|
3279 | |
---|
3280 | // We want to handle angular motors attached to passive geoms |
---|
3281 | dVector3 r; // axis[2] and axis[0] in global coordinates |
---|
3282 | r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3]; |
---|
3283 | dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); |
---|
3284 | dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); |
---|
3285 | j->reference2[0] += r[0]; j->reference2[1] += r[1]; |
---|
3286 | j->reference2[2] += r[2]; j->reference2[3] += r[3]; |
---|
3287 | } |
---|
3288 | } |
---|
3289 | |
---|
3290 | |
---|
3291 | static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) |
---|
3292 | { |
---|
3293 | info->m = 0; |
---|
3294 | info->nub = 0; |
---|
3295 | |
---|
3296 | // compute the axes and angles, if in euler mode |
---|
3297 | if (j->mode == dAMotorEuler) { |
---|
3298 | dVector3 ax[3]; |
---|
3299 | amotorComputeGlobalAxes (j,ax); |
---|
3300 | amotorComputeEulerAngles (j,ax); |
---|
3301 | } |
---|
3302 | |
---|
3303 | // see if we're powered or at a joint limit for each axis |
---|
3304 | for (int i=0; i < j->num; i++) { |
---|
3305 | if (j->limot[i].testRotationalLimit (j->angle[i]) || |
---|
3306 | j->limot[i].fmax > 0) { |
---|
3307 | info->m++; |
---|
3308 | } |
---|
3309 | } |
---|
3310 | } |
---|
3311 | |
---|
3312 | |
---|
3313 | static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) |
---|
3314 | { |
---|
3315 | int i; |
---|
3316 | |
---|
3317 | // compute the axes (if not global) |
---|
3318 | dVector3 ax[3]; |
---|
3319 | amotorComputeGlobalAxes (joint,ax); |
---|
3320 | |
---|
3321 | // in euler angle mode we do not actually constrain the angular velocity |
---|
3322 | // along the axes axis[0] and axis[2] (although we do use axis[1]) : |
---|
3323 | // |
---|
3324 | // to get constrain w2-w1 along ...not |
---|
3325 | // ------ --------------------- ------ |
---|
3326 | // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] |
---|
3327 | // d(angle[1])/dt = 0 ax[1] |
---|
3328 | // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] |
---|
3329 | // |
---|
3330 | // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. |
---|
3331 | // to prove the result for angle[0], write the expression for angle[0] from |
---|
3332 | // GetInfo1 then take the derivative. to prove this for angle[2] it is |
---|
3333 | // easier to take the euler rate expression for d(angle[2])/dt with respect |
---|
3334 | // to the components of w and set that to 0. |
---|
3335 | |
---|
3336 | dVector3 *axptr[3]; |
---|
3337 | axptr[0] = &ax[0]; |
---|
3338 | axptr[1] = &ax[1]; |
---|
3339 | axptr[2] = &ax[2]; |
---|
3340 | |
---|
3341 | dVector3 ax0_cross_ax1; |
---|
3342 | dVector3 ax1_cross_ax2; |
---|
3343 | if (joint->mode == dAMotorEuler) { |
---|
3344 | dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); |
---|
3345 | axptr[2] = &ax0_cross_ax1; |
---|
3346 | dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); |
---|
3347 | axptr[0] = &ax1_cross_ax2; |
---|
3348 | } |
---|
3349 | |
---|
3350 | int row=0; |
---|
3351 | for (i=0; i < joint->num; i++) { |
---|
3352 | row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); |
---|
3353 | } |
---|
3354 | } |
---|
3355 | |
---|
3356 | |
---|
3357 | void dJointSetAMotorNumAxes (dJointID j, int num) |
---|
3358 | { |
---|
3359 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3360 | dAASSERT(joint && num >= 0 && num <= 3); |
---|
3361 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3362 | if (joint->mode == dAMotorEuler) { |
---|
3363 | joint->num = 3; |
---|
3364 | } |
---|
3365 | else { |
---|
3366 | if (num < 0) num = 0; |
---|
3367 | if (num > 3) num = 3; |
---|
3368 | joint->num = num; |
---|
3369 | } |
---|
3370 | } |
---|
3371 | |
---|
3372 | |
---|
3373 | void dJointSetAMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) |
---|
3374 | { |
---|
3375 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3376 | dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); |
---|
3377 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3378 | dUASSERT(!(!joint->node[1].body && (joint->flags & dJOINT_REVERSE) && rel == 1),"no first body, can't set axis rel=1"); |
---|
3379 | dUASSERT(!(!joint->node[1].body && !(joint->flags & dJOINT_REVERSE) && rel == 2),"no second body, can't set axis rel=2"); |
---|
3380 | if (anum < 0) anum = 0; |
---|
3381 | if (anum > 2) anum = 2; |
---|
3382 | |
---|
3383 | // adjust rel to match the internal body order |
---|
3384 | if (!joint->node[1].body && rel==2) rel = 1; |
---|
3385 | |
---|
3386 | joint->rel[anum] = rel; |
---|
3387 | |
---|
3388 | // x,y,z is always in global coordinates regardless of rel, so we may have |
---|
3389 | // to convert it to be relative to a body |
---|
3390 | dVector3 r; |
---|
3391 | r[0] = x; |
---|
3392 | r[1] = y; |
---|
3393 | r[2] = z; |
---|
3394 | r[3] = 0; |
---|
3395 | if (rel > 0) { |
---|
3396 | if (rel==1) { |
---|
3397 | dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); |
---|
3398 | } |
---|
3399 | else { |
---|
3400 | // don't assert; handle the case of attachment to a bodiless geom |
---|
3401 | if (joint->node[1].body) { // jds |
---|
3402 | dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); |
---|
3403 | } |
---|
3404 | else { |
---|
3405 | joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; |
---|
3406 | joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3]; |
---|
3407 | } |
---|
3408 | } |
---|
3409 | } |
---|
3410 | else { |
---|
3411 | joint->axis[anum][0] = r[0]; |
---|
3412 | joint->axis[anum][1] = r[1]; |
---|
3413 | joint->axis[anum][2] = r[2]; |
---|
3414 | } |
---|
3415 | dNormalize3 (joint->axis[anum]); |
---|
3416 | if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); |
---|
3417 | } |
---|
3418 | |
---|
3419 | |
---|
3420 | void dJointSetAMotorAngle (dJointID j, int anum, dReal angle) |
---|
3421 | { |
---|
3422 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3423 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
3424 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3425 | if (joint->mode == dAMotorUser) { |
---|
3426 | if (anum < 0) anum = 0; |
---|
3427 | if (anum > 3) anum = 3; |
---|
3428 | joint->angle[anum] = angle; |
---|
3429 | } |
---|
3430 | } |
---|
3431 | |
---|
3432 | |
---|
3433 | void dJointSetAMotorParam (dJointID j, int parameter, dReal value) |
---|
3434 | { |
---|
3435 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3436 | dAASSERT(joint); |
---|
3437 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3438 | int anum = parameter >> 8; |
---|
3439 | if (anum < 0) anum = 0; |
---|
3440 | if (anum > 2) anum = 2; |
---|
3441 | parameter &= 0xff; |
---|
3442 | joint->limot[anum].set (parameter, value); |
---|
3443 | } |
---|
3444 | |
---|
3445 | |
---|
3446 | void dJointSetAMotorMode (dJointID j, int mode) |
---|
3447 | { |
---|
3448 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3449 | dAASSERT(joint); |
---|
3450 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3451 | joint->mode = mode; |
---|
3452 | if (joint->mode == dAMotorEuler) { |
---|
3453 | joint->num = 3; |
---|
3454 | amotorSetEulerReferenceVectors (joint); |
---|
3455 | } |
---|
3456 | } |
---|
3457 | |
---|
3458 | |
---|
3459 | int dJointGetAMotorNumAxes (dJointID j) |
---|
3460 | { |
---|
3461 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3462 | dAASSERT(joint); |
---|
3463 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3464 | return joint->num; |
---|
3465 | } |
---|
3466 | |
---|
3467 | |
---|
3468 | void dJointGetAMotorAxis (dJointID j, int anum, dVector3 result) |
---|
3469 | { |
---|
3470 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3471 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
3472 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3473 | if (anum < 0) anum = 0; |
---|
3474 | if (anum > 2) anum = 2; |
---|
3475 | if (joint->rel[anum] > 0) { |
---|
3476 | if (joint->rel[anum]==1) { |
---|
3477 | dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis[anum]); |
---|
3478 | } |
---|
3479 | else { |
---|
3480 | if (joint->node[1].body) { // jds |
---|
3481 | dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis[anum]); |
---|
3482 | } |
---|
3483 | else { |
---|
3484 | result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1]; |
---|
3485 | result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3]; |
---|
3486 | } |
---|
3487 | } |
---|
3488 | } |
---|
3489 | else { |
---|
3490 | result[0] = joint->axis[anum][0]; |
---|
3491 | result[1] = joint->axis[anum][1]; |
---|
3492 | result[2] = joint->axis[anum][2]; |
---|
3493 | } |
---|
3494 | } |
---|
3495 | |
---|
3496 | |
---|
3497 | int dJointGetAMotorAxisRel (dJointID j, int anum) |
---|
3498 | { |
---|
3499 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3500 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
3501 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3502 | if (anum < 0) anum = 0; |
---|
3503 | if (anum > 2) anum = 2; |
---|
3504 | return joint->rel[anum]; |
---|
3505 | } |
---|
3506 | |
---|
3507 | |
---|
3508 | dReal dJointGetAMotorAngle (dJointID j, int anum) |
---|
3509 | { |
---|
3510 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3511 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
3512 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3513 | if (anum < 0) anum = 0; |
---|
3514 | if (anum > 3) anum = 3; |
---|
3515 | return joint->angle[anum]; |
---|
3516 | } |
---|
3517 | |
---|
3518 | |
---|
3519 | dReal dJointGetAMotorAngleRate (dJointID j, int anum) |
---|
3520 | { |
---|
3521 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3522 | // @@@ |
---|
3523 | dDebug (0,"not yet implemented"); |
---|
3524 | return 0; |
---|
3525 | } |
---|
3526 | |
---|
3527 | |
---|
3528 | dReal dJointGetAMotorParam (dJointID j, int parameter) |
---|
3529 | { |
---|
3530 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3531 | dAASSERT(joint); |
---|
3532 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3533 | int anum = parameter >> 8; |
---|
3534 | if (anum < 0) anum = 0; |
---|
3535 | if (anum > 2) anum = 2; |
---|
3536 | parameter &= 0xff; |
---|
3537 | return joint->limot[anum].get (parameter); |
---|
3538 | } |
---|
3539 | |
---|
3540 | |
---|
3541 | int dJointGetAMotorMode (dJointID j) |
---|
3542 | { |
---|
3543 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3544 | dAASSERT(joint); |
---|
3545 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3546 | return joint->mode; |
---|
3547 | } |
---|
3548 | |
---|
3549 | |
---|
3550 | void dJointAddAMotorTorques (dJointID j, dReal torque1, dReal torque2, dReal torque3) |
---|
3551 | { |
---|
3552 | dxJointAMotor* joint = (dxJointAMotor*)j; |
---|
3553 | dVector3 axes[3]; |
---|
3554 | dAASSERT(joint); |
---|
3555 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
3556 | |
---|
3557 | if (joint->num == 0) |
---|
3558 | return; |
---|
3559 | dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); |
---|
3560 | |
---|
3561 | amotorComputeGlobalAxes (joint,axes); |
---|
3562 | axes[0][0] *= torque1; |
---|
3563 | axes[0][1] *= torque1; |
---|
3564 | axes[0][2] *= torque1; |
---|
3565 | if (joint->num >= 2) { |
---|
3566 | axes[0][0] += axes[1][0] * torque2; |
---|
3567 | axes[0][1] += axes[1][1] * torque2; |
---|
3568 | axes[0][2] += axes[1][2] * torque2; |
---|
3569 | if (joint->num >= 3) { |
---|
3570 | axes[0][0] += axes[2][0] * torque3; |
---|
3571 | axes[0][1] += axes[2][1] * torque3; |
---|
3572 | axes[0][2] += axes[2][2] * torque3; |
---|
3573 | } |
---|
3574 | } |
---|
3575 | |
---|
3576 | if (joint->node[0].body != 0) |
---|
3577 | dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); |
---|
3578 | if (joint->node[1].body != 0) |
---|
3579 | dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); |
---|
3580 | } |
---|
3581 | |
---|
3582 | |
---|
3583 | dxJoint::Vtable __damotor_vtable = { |
---|
3584 | sizeof(dxJointAMotor), |
---|
3585 | (dxJoint::init_fn*) amotorInit, |
---|
3586 | (dxJoint::getInfo1_fn*) amotorGetInfo1, |
---|
3587 | (dxJoint::getInfo2_fn*) amotorGetInfo2, |
---|
3588 | dJointTypeAMotor}; |
---|
3589 | |
---|
3590 | |
---|
3591 | |
---|
3592 | //**************************************************************************** |
---|
3593 | // lmotor joint |
---|
3594 | static void lmotorInit (dxJointLMotor *j) |
---|
3595 | { |
---|
3596 | int i; |
---|
3597 | j->num = 0; |
---|
3598 | for (i=0;i<3;i++) { |
---|
3599 | dSetZero(j->axis[i],4); |
---|
3600 | j->limot[i].init(j->world); |
---|
3601 | } |
---|
3602 | } |
---|
3603 | |
---|
3604 | static void lmotorComputeGlobalAxes (dxJointLMotor *joint, dVector3 ax[3]) |
---|
3605 | { |
---|
3606 | for (int i=0; i< joint->num; i++) { |
---|
3607 | if (joint->rel[i] == 1) { |
---|
3608 | dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); |
---|
3609 | } |
---|
3610 | else if (joint->rel[i] == 2) { |
---|
3611 | if (joint->node[1].body) { // jds: don't assert, just ignore |
---|
3612 | dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); |
---|
3613 | } |
---|
3614 | } else { |
---|
3615 | ax[i][0] = joint->axis[i][0]; |
---|
3616 | ax[i][1] = joint->axis[i][1]; |
---|
3617 | ax[i][2] = joint->axis[i][2]; |
---|
3618 | } |
---|
3619 | } |
---|
3620 | } |
---|
3621 | |
---|
3622 | static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info) |
---|
3623 | { |
---|
3624 | info->m = 0; |
---|
3625 | info->nub = 0; |
---|
3626 | for (int i=0; i < j->num; i++) { |
---|
3627 | if (j->limot[i].fmax > 0) { |
---|
3628 | info->m++; |
---|
3629 | } |
---|
3630 | } |
---|
3631 | } |
---|
3632 | |
---|
3633 | static void lmotorGetInfo2 (dxJointLMotor *joint, dxJoint::Info2 *info) |
---|
3634 | { |
---|
3635 | int row=0; |
---|
3636 | dVector3 ax[3]; |
---|
3637 | lmotorComputeGlobalAxes(joint, ax); |
---|
3638 | |
---|
3639 | for (int i=0;i<joint->num;i++) { |
---|
3640 | row += joint->limot[i].addLimot(joint,info,row,ax[i], 0); |
---|
3641 | } |
---|
3642 | } |
---|
3643 | |
---|
3644 | void dJointSetLMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) |
---|
3645 | { |
---|
3646 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3647 | //for now we are ignoring rel! |
---|
3648 | dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); |
---|
3649 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3650 | if (anum < 0) anum = 0; |
---|
3651 | if (anum > 2) anum = 2; |
---|
3652 | |
---|
3653 | if (!joint->node[1].body && rel==2) rel = 1; //ref 1 |
---|
3654 | |
---|
3655 | joint->rel[anum] = rel; |
---|
3656 | |
---|
3657 | dVector3 r; |
---|
3658 | r[0] = x; |
---|
3659 | r[1] = y; |
---|
3660 | r[2] = z; |
---|
3661 | r[3] = 0; |
---|
3662 | if (rel > 0) { |
---|
3663 | if (rel==1) { |
---|
3664 | dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); |
---|
3665 | } else { |
---|
3666 | //second body has to exists thanks to ref 1 line |
---|
3667 | dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); |
---|
3668 | } |
---|
3669 | } else { |
---|
3670 | joint->axis[anum][0] = r[0]; |
---|
3671 | joint->axis[anum][1] = r[1]; |
---|
3672 | joint->axis[anum][2] = r[2]; |
---|
3673 | } |
---|
3674 | |
---|
3675 | dNormalize3 (joint->axis[anum]); |
---|
3676 | } |
---|
3677 | |
---|
3678 | void dJointSetLMotorNumAxes (dJointID j, int num) |
---|
3679 | { |
---|
3680 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3681 | dAASSERT(joint && num >= 0 && num <= 3); |
---|
3682 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3683 | if (num < 0) num = 0; |
---|
3684 | if (num > 3) num = 3; |
---|
3685 | joint->num = num; |
---|
3686 | } |
---|
3687 | |
---|
3688 | void dJointSetLMotorParam (dJointID j, int parameter, dReal value) |
---|
3689 | { |
---|
3690 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3691 | dAASSERT(joint); |
---|
3692 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3693 | int anum = parameter >> 8; |
---|
3694 | if (anum < 0) anum = 0; |
---|
3695 | if (anum > 2) anum = 2; |
---|
3696 | parameter &= 0xff; |
---|
3697 | joint->limot[anum].set (parameter, value); |
---|
3698 | } |
---|
3699 | |
---|
3700 | int dJointGetLMotorNumAxes (dJointID j) |
---|
3701 | { |
---|
3702 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3703 | dAASSERT(joint); |
---|
3704 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3705 | return joint->num; |
---|
3706 | } |
---|
3707 | |
---|
3708 | |
---|
3709 | void dJointGetLMotorAxis (dJointID j, int anum, dVector3 result) |
---|
3710 | { |
---|
3711 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3712 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
3713 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3714 | if (anum < 0) anum = 0; |
---|
3715 | if (anum > 2) anum = 2; |
---|
3716 | result[0] = joint->axis[anum][0]; |
---|
3717 | result[1] = joint->axis[anum][1]; |
---|
3718 | result[2] = joint->axis[anum][2]; |
---|
3719 | } |
---|
3720 | |
---|
3721 | dReal dJointGetLMotorParam (dJointID j, int parameter) |
---|
3722 | { |
---|
3723 | dxJointLMotor* joint = (dxJointLMotor*)j; |
---|
3724 | dAASSERT(joint); |
---|
3725 | dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); |
---|
3726 | int anum = parameter >> 8; |
---|
3727 | if (anum < 0) anum = 0; |
---|
3728 | if (anum > 2) anum = 2; |
---|
3729 | parameter &= 0xff; |
---|
3730 | return joint->limot[anum].get (parameter); |
---|
3731 | } |
---|
3732 | |
---|
3733 | dxJoint::Vtable __dlmotor_vtable = { |
---|
3734 | sizeof(dxJointLMotor), |
---|
3735 | (dxJoint::init_fn*) lmotorInit, |
---|
3736 | (dxJoint::getInfo1_fn*) lmotorGetInfo1, |
---|
3737 | (dxJoint::getInfo2_fn*) lmotorGetInfo2, |
---|
3738 | dJointTypeLMotor |
---|
3739 | }; |
---|
3740 | |
---|
3741 | |
---|
3742 | //**************************************************************************** |
---|
3743 | // fixed joint |
---|
3744 | |
---|
3745 | static void fixedInit (dxJointFixed *j) |
---|
3746 | { |
---|
3747 | dSetZero (j->offset,4); |
---|
3748 | dSetZero (j->qrel,4); |
---|
3749 | j->erp = j->world->global_erp; |
---|
3750 | j->cfm = j->world->global_cfm; |
---|
3751 | } |
---|
3752 | |
---|
3753 | |
---|
3754 | static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) |
---|
3755 | { |
---|
3756 | info->m = 6; |
---|
3757 | info->nub = 6; |
---|
3758 | } |
---|
3759 | |
---|
3760 | |
---|
3761 | static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) |
---|
3762 | { |
---|
3763 | int s = info->rowskip; |
---|
3764 | |
---|
3765 | // Three rows for orientation |
---|
3766 | setFixedOrientation(joint, info, joint->qrel, 3); |
---|
3767 | |
---|
3768 | // Three rows for position. |
---|
3769 | // set jacobian |
---|
3770 | info->J1l[0] = 1; |
---|
3771 | info->J1l[s+1] = 1; |
---|
3772 | info->J1l[2*s+2] = 1; |
---|
3773 | |
---|
3774 | info->erp = joint->erp; |
---|
3775 | info->cfm[0] = joint->cfm; |
---|
3776 | info->cfm[1] = joint->cfm; |
---|
3777 | info->cfm[2] = joint->cfm; |
---|
3778 | |
---|
3779 | dVector3 ofs; |
---|
3780 | dMULTIPLY0_331 (ofs,joint->node[0].body->posr.R,joint->offset); |
---|
3781 | if (joint->node[1].body) { |
---|
3782 | dCROSSMAT (info->J1a,ofs,s,+,-); |
---|
3783 | info->J2l[0] = -1; |
---|
3784 | info->J2l[s+1] = -1; |
---|
3785 | info->J2l[2*s+2] = -1; |
---|
3786 | } |
---|
3787 | |
---|
3788 | // set right hand side for the first three rows (linear) |
---|
3789 | dReal k = info->fps * info->erp; |
---|
3790 | if (joint->node[1].body) { |
---|
3791 | for (int j=0; j<3; j++) |
---|
3792 | info->c[j] = k * (joint->node[1].body->posr.pos[j] - |
---|
3793 | joint->node[0].body->posr.pos[j] + ofs[j]); |
---|
3794 | } |
---|
3795 | else { |
---|
3796 | for (int j=0; j<3; j++) |
---|
3797 | info->c[j] = k * (joint->offset[j] - joint->node[0].body->posr.pos[j]); |
---|
3798 | } |
---|
3799 | } |
---|
3800 | |
---|
3801 | |
---|
3802 | void dJointSetFixed (dJointID j) |
---|
3803 | { |
---|
3804 | dxJointFixed* joint = (dxJointFixed*)j; |
---|
3805 | dUASSERT(joint,"bad joint argument"); |
---|
3806 | dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); |
---|
3807 | int i; |
---|
3808 | |
---|
3809 | // This code is taken from sJointSetSliderAxis(), we should really put the |
---|
3810 | // common code in its own function. |
---|
3811 | // compute the offset between the bodies |
---|
3812 | if (joint->node[0].body) { |
---|
3813 | if (joint->node[1].body) { |
---|
3814 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
3815 | dReal ofs[4]; |
---|
3816 | for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i]; |
---|
3817 | for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i]; |
---|
3818 | dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs); |
---|
3819 | } |
---|
3820 | else { |
---|
3821 | // set joint->qrel to the transpose of the first body's q |
---|
3822 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
3823 | for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
3824 | for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; |
---|
3825 | } |
---|
3826 | } |
---|
3827 | } |
---|
3828 | |
---|
3829 | void dxJointFixed::set (int num, dReal value) |
---|
3830 | { |
---|
3831 | switch (num) { |
---|
3832 | case dParamCFM: |
---|
3833 | cfm = value; |
---|
3834 | break; |
---|
3835 | case dParamERP: |
---|
3836 | erp = value; |
---|
3837 | break; |
---|
3838 | } |
---|
3839 | } |
---|
3840 | |
---|
3841 | |
---|
3842 | dReal dxJointFixed::get (int num) |
---|
3843 | { |
---|
3844 | switch (num) { |
---|
3845 | case dParamCFM: |
---|
3846 | return cfm; |
---|
3847 | case dParamERP: |
---|
3848 | return erp; |
---|
3849 | default: |
---|
3850 | return 0; |
---|
3851 | } |
---|
3852 | } |
---|
3853 | |
---|
3854 | |
---|
3855 | void dJointSetFixedParam (dJointID j, int parameter, dReal value) |
---|
3856 | { |
---|
3857 | dxJointFixed* joint = (dxJointFixed*)j; |
---|
3858 | dUASSERT(joint,"bad joint argument"); |
---|
3859 | dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint"); |
---|
3860 | joint->set (parameter,value); |
---|
3861 | } |
---|
3862 | |
---|
3863 | |
---|
3864 | dReal dJointGetFixedParam (dJointID j, int parameter) |
---|
3865 | { |
---|
3866 | dxJointFixed* joint = (dxJointFixed*)j; |
---|
3867 | dUASSERT(joint,"bad joint argument"); |
---|
3868 | dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint"); |
---|
3869 | return joint->get (parameter); |
---|
3870 | } |
---|
3871 | |
---|
3872 | |
---|
3873 | dxJoint::Vtable __dfixed_vtable = { |
---|
3874 | sizeof(dxJointFixed), |
---|
3875 | (dxJoint::init_fn*) fixedInit, |
---|
3876 | (dxJoint::getInfo1_fn*) fixedGetInfo1, |
---|
3877 | (dxJoint::getInfo2_fn*) fixedGetInfo2, |
---|
3878 | dJointTypeFixed}; |
---|
3879 | |
---|
3880 | //**************************************************************************** |
---|
3881 | // null joint |
---|
3882 | |
---|
3883 | static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) |
---|
3884 | { |
---|
3885 | info->m = 0; |
---|
3886 | info->nub = 0; |
---|
3887 | } |
---|
3888 | |
---|
3889 | |
---|
3890 | static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) |
---|
3891 | { |
---|
3892 | dDebug (0,"this should never get called"); |
---|
3893 | } |
---|
3894 | |
---|
3895 | |
---|
3896 | dxJoint::Vtable __dnull_vtable = { |
---|
3897 | sizeof(dxJointNull), |
---|
3898 | (dxJoint::init_fn*) 0, |
---|
3899 | (dxJoint::getInfo1_fn*) nullGetInfo1, |
---|
3900 | (dxJoint::getInfo2_fn*) nullGetInfo2, |
---|
3901 | dJointTypeNull}; |
---|
3902 | |
---|
3903 | |
---|
3904 | |
---|
3905 | |
---|
3906 | /* |
---|
3907 | This code is part of the Plane2D ODE joint |
---|
3908 | by psero@gmx.de |
---|
3909 | Wed Apr 23 18:53:43 CEST 2003 |
---|
3910 | |
---|
3911 | Add this code to the file: ode/src/joint.cpp |
---|
3912 | */ |
---|
3913 | |
---|
3914 | |
---|
3915 | # define VoXYZ(v1, o1, x, y, z) \ |
---|
3916 | ( \ |
---|
3917 | (v1)[0] o1 (x), \ |
---|
3918 | (v1)[1] o1 (y), \ |
---|
3919 | (v1)[2] o1 (z) \ |
---|
3920 | ) |
---|
3921 | |
---|
3922 | static dReal Midentity[3][3] = |
---|
3923 | { |
---|
3924 | { 1, 0, 0 }, |
---|
3925 | { 0, 1, 0 }, |
---|
3926 | { 0, 0, 1, } |
---|
3927 | }; |
---|
3928 | |
---|
3929 | |
---|
3930 | |
---|
3931 | static void plane2dInit (dxJointPlane2D *j) |
---|
3932 | /*********************************************/ |
---|
3933 | { |
---|
3934 | /* MINFO ("plane2dInit ()"); */ |
---|
3935 | j->motor_x.init (j->world); |
---|
3936 | j->motor_y.init (j->world); |
---|
3937 | j->motor_angle.init (j->world); |
---|
3938 | } |
---|
3939 | |
---|
3940 | |
---|
3941 | |
---|
3942 | static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info) |
---|
3943 | /***********************************************************************/ |
---|
3944 | { |
---|
3945 | /* MINFO ("plane2dGetInfo1 ()"); */ |
---|
3946 | |
---|
3947 | info->nub = 3; |
---|
3948 | info->m = 3; |
---|
3949 | |
---|
3950 | if (j->motor_x.fmax > 0) |
---|
3951 | j->row_motor_x = info->m ++; |
---|
3952 | if (j->motor_y.fmax > 0) |
---|
3953 | j->row_motor_y = info->m ++; |
---|
3954 | if (j->motor_angle.fmax > 0) |
---|
3955 | j->row_motor_angle = info->m ++; |
---|
3956 | } |
---|
3957 | |
---|
3958 | |
---|
3959 | |
---|
3960 | static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info) |
---|
3961 | /***************************************************************************/ |
---|
3962 | { |
---|
3963 | int r0 = 0, |
---|
3964 | r1 = info->rowskip, |
---|
3965 | r2 = 2 * r1; |
---|
3966 | dReal eps = info->fps * info->erp; |
---|
3967 | |
---|
3968 | /* MINFO ("plane2dGetInfo2 ()"); */ |
---|
3969 | |
---|
3970 | /* |
---|
3971 | v = v1, w = omega1 |
---|
3972 | (v2, omega2 not important (== static environment)) |
---|
3973 | |
---|
3974 | constraint equations: |
---|
3975 | xz = 0 |
---|
3976 | wx = 0 |
---|
3977 | wy = 0 |
---|
3978 | |
---|
3979 | <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 ) |
---|
3980 | ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 ) |
---|
3981 | ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 ) |
---|
3982 | J1/J1l Omega1/J1a |
---|
3983 | */ |
---|
3984 | |
---|
3985 | // fill in linear and angular coeff. for left hand side: |
---|
3986 | |
---|
3987 | VoXYZ (&info->J1l[r0], =, 0, 0, 1); |
---|
3988 | VoXYZ (&info->J1l[r1], =, 0, 0, 0); |
---|
3989 | VoXYZ (&info->J1l[r2], =, 0, 0, 0); |
---|
3990 | |
---|
3991 | VoXYZ (&info->J1a[r0], =, 0, 0, 0); |
---|
3992 | VoXYZ (&info->J1a[r1], =, 1, 0, 0); |
---|
3993 | VoXYZ (&info->J1a[r2], =, 0, 1, 0); |
---|
3994 | |
---|
3995 | // error correction (against drift): |
---|
3996 | |
---|
3997 | // a) linear vz, so that z (== pos[2]) == 0 |
---|
3998 | info->c[0] = eps * -joint->node[0].body->posr.pos[2]; |
---|
3999 | |
---|
4000 | # if 0 |
---|
4001 | // b) angular correction? -> left to application !!! |
---|
4002 | dReal *body_z_axis = &joint->node[0].body->R[8]; |
---|
4003 | info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error |
---|
4004 | info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error |
---|
4005 | # endif |
---|
4006 | |
---|
4007 | // if the slider is powered, or has joint limits, add in the extra row: |
---|
4008 | |
---|
4009 | if (joint->row_motor_x > 0) |
---|
4010 | joint->motor_x.addLimot ( |
---|
4011 | joint, info, joint->row_motor_x, Midentity[0], 0); |
---|
4012 | |
---|
4013 | if (joint->row_motor_y > 0) |
---|
4014 | joint->motor_y.addLimot ( |
---|
4015 | joint, info, joint->row_motor_y, Midentity[1], 0); |
---|
4016 | |
---|
4017 | if (joint->row_motor_angle > 0) |
---|
4018 | joint->motor_angle.addLimot ( |
---|
4019 | joint, info, joint->row_motor_angle, Midentity[2], 1); |
---|
4020 | } |
---|
4021 | |
---|
4022 | |
---|
4023 | |
---|
4024 | dxJoint::Vtable __dplane2d_vtable = |
---|
4025 | { |
---|
4026 | sizeof (dxJointPlane2D), |
---|
4027 | (dxJoint::init_fn*) plane2dInit, |
---|
4028 | (dxJoint::getInfo1_fn*) plane2dGetInfo1, |
---|
4029 | (dxJoint::getInfo2_fn*) plane2dGetInfo2, |
---|
4030 | dJointTypePlane2D |
---|
4031 | }; |
---|
4032 | |
---|
4033 | |
---|
4034 | void dJointSetPlane2DXParam (dxJoint *joint, |
---|
4035 | int parameter, dReal value) |
---|
4036 | { |
---|
4037 | dUASSERT (joint, "bad joint argument"); |
---|
4038 | dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); |
---|
4039 | dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
---|
4040 | joint2d->motor_x.set (parameter, value); |
---|
4041 | } |
---|
4042 | |
---|
4043 | |
---|
4044 | void dJointSetPlane2DYParam (dxJoint *joint, |
---|
4045 | int parameter, dReal value) |
---|
4046 | { |
---|
4047 | dUASSERT (joint, "bad joint argument"); |
---|
4048 | dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); |
---|
4049 | dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
---|
4050 | joint2d->motor_y.set (parameter, value); |
---|
4051 | } |
---|
4052 | |
---|
4053 | |
---|
4054 | |
---|
4055 | void dJointSetPlane2DAngleParam (dxJoint *joint, |
---|
4056 | int parameter, dReal value) |
---|
4057 | { |
---|
4058 | dUASSERT (joint, "bad joint argument"); |
---|
4059 | dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); |
---|
4060 | dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
---|
4061 | joint2d->motor_angle.set (parameter, value); |
---|
4062 | } |
---|
4063 | |
---|
4064 | |
---|
4065 | |
---|