|
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 #include "object.h" |
|
24 #include "joint.h" |
|
25 #include <ode/config.h> |
|
26 #include <ode/odemath.h> |
|
27 #include <ode/rotation.h> |
|
28 #include <ode/timer.h> |
|
29 #include <ode/error.h> |
|
30 #include <ode/matrix.h> |
|
31 #include "lcp.h" |
|
32 #include "util.h" |
|
33 |
|
34 //**************************************************************************** |
|
35 // misc defines |
|
36 |
|
37 // memory allocation system |
|
38 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
39 unsigned int dMemoryFlag; |
|
40 #define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation. Results will not be accurate.\n") |
|
41 |
|
42 #define ALLOCA(t,v,s) t* v=(t*)malloc(s) |
|
43 #define UNALLOCA(t) free(t) |
|
44 |
|
45 #else |
|
46 #define ALLOCA(t,v,s) t* v=(t*)dALLOCA16(s) |
|
47 #define UNALLOCA(t) /* nothing */ |
|
48 #endif |
|
49 |
|
50 |
|
51 // undef to use the fast decomposition |
|
52 #define DIRECT_CHOLESKY |
|
53 |
|
54 //**************************************************************************** |
|
55 // special matrix multipliers |
|
56 |
|
57 // this assumes the 4th and 8th rows of B and C are zero. |
|
58 |
|
59 static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, |
|
60 int p, int r, int Askip) |
|
61 { |
|
62 int i,j; |
|
63 dReal sum,*bb,*cc; |
|
64 |
|
65 bb = B; |
|
66 for (i=p; i; i--) { |
|
67 cc = C; |
|
68 for (j=r; j; j--) { |
|
69 sum = dMUL(bb[0],cc[0]); |
|
70 sum += dMUL(bb[1],cc[1]); |
|
71 sum += dMUL(bb[2],cc[2]); |
|
72 sum += dMUL(bb[4],cc[4]); |
|
73 sum += dMUL(bb[5],cc[5]); |
|
74 sum += dMUL(bb[6],cc[6]); |
|
75 *(A++) = sum; |
|
76 cc += 8; |
|
77 } |
|
78 A += Askip - r; |
|
79 bb += 8; |
|
80 } |
|
81 } |
|
82 |
|
83 |
|
84 // this assumes the 4th and 8th rows of B and C are zero. |
|
85 |
|
86 static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, |
|
87 int p, int r, int Askip) |
|
88 { |
|
89 int i,j; |
|
90 dReal sum,*bb,*cc; |
|
91 |
|
92 bb = B; |
|
93 for (i=p; i; i--) { |
|
94 cc = C; |
|
95 for (j=r; j; j--) { |
|
96 sum = dMUL(bb[0],cc[0]); |
|
97 sum += dMUL(bb[1],cc[1]); |
|
98 sum += dMUL(bb[2],cc[2]); |
|
99 sum += dMUL(bb[4],cc[4]); |
|
100 sum += dMUL(bb[5],cc[5]); |
|
101 sum += dMUL(bb[6],cc[6]); |
|
102 *(A++) += sum; |
|
103 cc += 8; |
|
104 } |
|
105 A += Askip - r; |
|
106 bb += 8; |
|
107 } |
|
108 } |
|
109 |
|
110 |
|
111 // this assumes the 4th and 8th rows of B are zero. |
|
112 |
|
113 static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) |
|
114 { |
|
115 int i; |
|
116 |
|
117 dReal sum; |
|
118 for (i=p; i; i--) { |
|
119 sum = dMUL(B[0],C[0]); |
|
120 sum += dMUL(B[1],C[1]); |
|
121 sum += dMUL(B[2],C[2]); |
|
122 sum += dMUL(B[4],C[4]); |
|
123 sum += dMUL(B[5],C[5]); |
|
124 sum += dMUL(B[6],C[6]); |
|
125 *(A++) = sum; |
|
126 B += 8; |
|
127 } |
|
128 } |
|
129 |
|
130 |
|
131 // this assumes the 4th and 8th rows of B are zero. |
|
132 |
|
133 static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) |
|
134 { |
|
135 int i; |
|
136 |
|
137 dReal sum; |
|
138 for (i=p; i; i--) { |
|
139 sum = dMUL(B[0],C[0]); |
|
140 sum += dMUL(B[1],C[1]); |
|
141 sum += dMUL(B[2],C[2]); |
|
142 sum += dMUL(B[4],C[4]); |
|
143 sum += dMUL(B[5],C[5]); |
|
144 sum += dMUL(B[6],C[6]); |
|
145 *(A++) += sum; |
|
146 B += 8; |
|
147 } |
|
148 } |
|
149 |
|
150 |
|
151 // this assumes the 4th and 8th rows of B are zero. |
|
152 |
|
153 static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) |
|
154 { |
|
155 int k; |
|
156 dReal sum; |
|
157 |
|
158 sum = 0; |
|
159 for (k=0; k<q; k++) sum += dMUL(B[k*8],C[k]); |
|
160 A[0] += sum; |
|
161 sum = 0; |
|
162 for (k=0; k<q; k++) sum += dMUL(B[1+k*8],C[k]); |
|
163 A[1] += sum; |
|
164 sum = 0; |
|
165 for (k=0; k<q; k++) sum += dMUL(B[2+k*8],C[k]); |
|
166 A[2] += sum; |
|
167 sum = 0; |
|
168 for (k=0; k<q; k++) sum += dMUL(B[4+k*8],C[k]); |
|
169 A[4] += sum; |
|
170 sum = 0; |
|
171 for (k=0; k<q; k++) sum += dMUL(B[5+k*8],C[k]); |
|
172 A[5] += sum; |
|
173 sum = 0; |
|
174 for (k=0; k<q; k++) sum += dMUL(B[6+k*8],C[k]); |
|
175 A[6] += sum; |
|
176 } |
|
177 |
|
178 |
|
179 // this assumes the 4th and 8th rows of B are zero. |
|
180 |
|
181 static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q) |
|
182 { |
|
183 int k; |
|
184 dReal sum; |
|
185 |
|
186 sum = 0; |
|
187 for (k=0; k<q; k++) sum += dMUL(B[k*8],C[k]); |
|
188 A[0] = sum; |
|
189 sum = 0; |
|
190 for (k=0; k<q; k++) sum += dMUL(B[1+k*8],C[k]); |
|
191 A[1] = sum; |
|
192 sum = 0; |
|
193 for (k=0; k<q; k++) sum += dMUL(B[2+k*8],C[k]); |
|
194 A[2] = sum; |
|
195 sum = 0; |
|
196 for (k=0; k<q; k++) sum += dMUL(B[4+k*8],C[k]); |
|
197 A[4] = sum; |
|
198 sum = 0; |
|
199 for (k=0; k<q; k++) sum += dMUL(B[5+k*8],C[k]); |
|
200 A[5] = sum; |
|
201 sum = 0; |
|
202 for (k=0; k<q; k++) sum += dMUL(B[6+k*8],C[k]); |
|
203 A[6] = sum; |
|
204 } |
|
205 |
|
206 //**************************************************************************** |
|
207 // the slow, but sure way |
|
208 // note that this does not do any joint feedback! |
|
209 |
|
210 // given lists of bodies and joints that form an island, perform a first |
|
211 // order timestep. |
|
212 // |
|
213 // `body' is the body array, `nb' is the size of the array. |
|
214 // `_joint' is the body array, `nj' is the size of the array. |
|
215 |
|
216 void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb, |
|
217 dxJoint * const *_joint, int nj, dReal stepsize) |
|
218 { |
|
219 int i,j,k; |
|
220 int n6 = 6*nb; |
|
221 |
|
222 |
|
223 // number all bodies in the body list - set their tag values |
|
224 for (i=0; i<nb; i++) body[i]->tag = i; |
|
225 |
|
226 // make a local copy of the joint array, because we might want to modify it. |
|
227 // (the "dxJoint *const*" declaration says we're allowed to modify the joints |
|
228 // but not the joint array, because the caller might need it unchanged). |
|
229 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); |
|
230 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
231 if (joint == NULL) { |
|
232 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
233 return; |
|
234 } |
|
235 #endif |
|
236 memcpy (joint,_joint,nj * sizeof(dxJoint*)); |
|
237 |
|
238 // for all bodies, compute the inertia tensor and its inverse in the global |
|
239 // frame, and compute the rotational force and add it to the torque |
|
240 // accumulator. |
|
241 // @@@ check computation of rotational force. |
|
242 ALLOCA(dReal,I,3*nb*4*sizeof(dReal)); |
|
243 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
244 if (I == NULL) { |
|
245 UNALLOCA(joint); |
|
246 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
247 return; |
|
248 } |
|
249 #endif |
|
250 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); |
|
251 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
252 if (invI == NULL) { |
|
253 UNALLOCA(I); |
|
254 UNALLOCA(joint); |
|
255 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
256 return; |
|
257 } |
|
258 #endif |
|
259 |
|
260 //dSetZero (I,3*nb*4); |
|
261 //dSetZero (invI,3*nb*4); |
|
262 for (i=0; i<nb; i++) { |
|
263 dReal tmp[12]; |
|
264 // compute inertia tensor in global frame |
|
265 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R); |
|
266 dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); |
|
267 // compute inverse inertia tensor in global frame |
|
268 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R); |
|
269 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); |
|
270 // compute rotational force |
|
271 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); |
|
272 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); |
|
273 } |
|
274 |
|
275 // add the gravity force to all bodies |
|
276 for (i=0; i<nb; i++) { |
|
277 if ((body[i]->flags & dxBodyNoGravity)==0) { |
|
278 body[i]->facc[0] += dMUL(body[i]->mass.mass,world->gravity[0]); |
|
279 body[i]->facc[1] += dMUL(body[i]->mass.mass,world->gravity[1]); |
|
280 body[i]->facc[2] += dMUL(body[i]->mass.mass,world->gravity[2]); |
|
281 } |
|
282 } |
|
283 |
|
284 // get m = total constraint dimension, nub = number of unbounded variables. |
|
285 // create constraint offset array and number-of-rows array for all joints. |
|
286 // the constraints are re-ordered as follows: the purely unbounded |
|
287 // constraints, the mixed unbounded + LCP constraints, and last the purely |
|
288 // LCP constraints. |
|
289 // |
|
290 // joints with m=0 are inactive and are removed from the joints array |
|
291 // entirely, so that the code that follows does not consider them. |
|
292 int m = 0; |
|
293 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); |
|
294 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
295 if (info == NULL) { |
|
296 UNALLOCA(invI); |
|
297 UNALLOCA(I); |
|
298 UNALLOCA(joint); |
|
299 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
300 return; |
|
301 } |
|
302 #endif |
|
303 |
|
304 ALLOCA(int,ofs,nj*sizeof(int)); |
|
305 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
306 if (ofs == NULL) { |
|
307 UNALLOCA(info); |
|
308 UNALLOCA(invI); |
|
309 UNALLOCA(I); |
|
310 UNALLOCA(joint); |
|
311 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
312 return; |
|
313 } |
|
314 #endif |
|
315 |
|
316 for (i=0, j=0; j<nj; j++) { // i=dest, j=src |
|
317 joint[j]->vtable->getInfo1 (joint[j],info+i); |
|
318 |
|
319 if (info[i].m > 0) { |
|
320 joint[i] = joint[j]; |
|
321 i++; |
|
322 } |
|
323 } |
|
324 nj = i; |
|
325 |
|
326 // the purely unbounded constraints |
|
327 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) { |
|
328 ofs[i] = m; |
|
329 m += info[i].m; |
|
330 } |
|
331 // the mixed unbounded + LCP constraints |
|
332 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) { |
|
333 ofs[i] = m; |
|
334 m += info[i].m; |
|
335 } |
|
336 // the purely LCP constraints |
|
337 for (i=0; i<nj; i++) if (info[i].nub == 0) { |
|
338 ofs[i] = m; |
|
339 m += info[i].m; |
|
340 } |
|
341 |
|
342 // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass |
|
343 // parameters |
|
344 int nskip = dPAD (n6); |
|
345 ALLOCA(dReal, invM, n6*nskip*sizeof(dReal)); |
|
346 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
347 if (invM == NULL) { |
|
348 UNALLOCA(ofs); |
|
349 UNALLOCA(info); |
|
350 UNALLOCA(invI); |
|
351 UNALLOCA(I); |
|
352 UNALLOCA(joint); |
|
353 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
354 return; |
|
355 } |
|
356 #endif |
|
357 |
|
358 dSetZero (invM,n6*nskip); |
|
359 for (i=0; i<nb; i++) { |
|
360 dReal *MM = invM+(i*6)*nskip+(i*6); |
|
361 MM[0] = body[i]->invMass; |
|
362 MM[nskip+1] = body[i]->invMass; |
|
363 MM[2*nskip+2] = body[i]->invMass; |
|
364 MM += 3*nskip+3; |
|
365 for (j=0; j<3; j++) for (k=0; k<3; k++) { |
|
366 MM[j*nskip+k] = invI[i*12+j*4+k]; |
|
367 } |
|
368 } |
|
369 |
|
370 // assemble some body vectors: fe = external forces, v = velocities |
|
371 ALLOCA(dReal,fe,n6*sizeof(dReal)); |
|
372 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
373 if (fe == NULL) { |
|
374 UNALLOCA(invM); |
|
375 UNALLOCA(ofs); |
|
376 UNALLOCA(info); |
|
377 UNALLOCA(invI); |
|
378 UNALLOCA(I); |
|
379 UNALLOCA(joint); |
|
380 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
381 return; |
|
382 } |
|
383 #endif |
|
384 |
|
385 ALLOCA(dReal,v,n6*sizeof(dReal)); |
|
386 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
387 if (v == NULL) { |
|
388 UNALLOCA(fe); |
|
389 UNALLOCA(invM); |
|
390 UNALLOCA(ofs); |
|
391 UNALLOCA(info); |
|
392 UNALLOCA(invI); |
|
393 UNALLOCA(I); |
|
394 UNALLOCA(joint); |
|
395 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
396 return; |
|
397 } |
|
398 #endif |
|
399 |
|
400 //dSetZero (fe,n6); |
|
401 //dSetZero (v,n6); |
|
402 for (i=0; i<nb; i++) { |
|
403 for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j]; |
|
404 for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; |
|
405 for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; |
|
406 for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; |
|
407 } |
|
408 |
|
409 // this will be set to the velocity update |
|
410 ALLOCA(dReal,vnew,n6*sizeof(dReal)); |
|
411 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
412 if (vnew == NULL) { |
|
413 UNALLOCA(v); |
|
414 UNALLOCA(fe); |
|
415 UNALLOCA(invM); |
|
416 UNALLOCA(ofs); |
|
417 UNALLOCA(info); |
|
418 UNALLOCA(invI); |
|
419 UNALLOCA(I); |
|
420 UNALLOCA(joint); |
|
421 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
422 return; |
|
423 } |
|
424 #endif |
|
425 dSetZero (vnew,n6); |
|
426 |
|
427 // if there are constraints, compute cforce |
|
428 if (m > 0) { |
|
429 // create a constraint equation right hand side vector `c', a constraint |
|
430 // force mixing vector `cfm', and LCP low and high bound vectors, and an |
|
431 // 'findex' vector. |
|
432 ALLOCA(dReal,c,m*sizeof(dReal)); |
|
433 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
434 if (c == NULL) { |
|
435 UNALLOCA(vnew); |
|
436 UNALLOCA(v); |
|
437 UNALLOCA(fe); |
|
438 UNALLOCA(invM); |
|
439 UNALLOCA(ofs); |
|
440 UNALLOCA(info); |
|
441 UNALLOCA(invI); |
|
442 UNALLOCA(I); |
|
443 UNALLOCA(joint); |
|
444 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
445 return; |
|
446 } |
|
447 #endif |
|
448 ALLOCA(dReal,cfm,m*sizeof(dReal)); |
|
449 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
450 if (cfm == NULL) { |
|
451 UNALLOCA(c); |
|
452 UNALLOCA(vnew); |
|
453 UNALLOCA(v); |
|
454 UNALLOCA(fe); |
|
455 UNALLOCA(invM); |
|
456 UNALLOCA(ofs); |
|
457 UNALLOCA(info); |
|
458 UNALLOCA(invI); |
|
459 UNALLOCA(I); |
|
460 UNALLOCA(joint); |
|
461 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
462 return; |
|
463 } |
|
464 #endif |
|
465 ALLOCA(dReal,lo,m*sizeof(dReal)); |
|
466 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
467 if (lo == NULL) { |
|
468 UNALLOCA(cfm); |
|
469 UNALLOCA(c); |
|
470 UNALLOCA(vnew); |
|
471 UNALLOCA(v); |
|
472 UNALLOCA(fe); |
|
473 UNALLOCA(invM); |
|
474 UNALLOCA(ofs); |
|
475 UNALLOCA(info); |
|
476 UNALLOCA(invI); |
|
477 UNALLOCA(I); |
|
478 UNALLOCA(joint); |
|
479 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
480 return; |
|
481 } |
|
482 #endif |
|
483 ALLOCA(dReal,hi,m*sizeof(dReal)); |
|
484 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
485 if (hi == NULL) { |
|
486 UNALLOCA(lo); |
|
487 UNALLOCA(cfm); |
|
488 UNALLOCA(c); |
|
489 UNALLOCA(vnew); |
|
490 UNALLOCA(v); |
|
491 UNALLOCA(fe); |
|
492 UNALLOCA(invM); |
|
493 UNALLOCA(ofs); |
|
494 UNALLOCA(info); |
|
495 UNALLOCA(invI); |
|
496 UNALLOCA(I); |
|
497 UNALLOCA(joint); |
|
498 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
499 return; |
|
500 } |
|
501 #endif |
|
502 ALLOCA(int,findex,m*sizeof(int)); |
|
503 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
504 if (findex == NULL) { |
|
505 UNALLOCA(hi); |
|
506 UNALLOCA(lo); |
|
507 UNALLOCA(cfm); |
|
508 UNALLOCA(c); |
|
509 UNALLOCA(vnew); |
|
510 UNALLOCA(v); |
|
511 UNALLOCA(fe); |
|
512 UNALLOCA(invM); |
|
513 UNALLOCA(ofs); |
|
514 UNALLOCA(info); |
|
515 UNALLOCA(invI); |
|
516 UNALLOCA(I); |
|
517 UNALLOCA(joint); |
|
518 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
519 return; |
|
520 } |
|
521 #endif |
|
522 dSetZero (c,m); |
|
523 dSetValue (cfm,m,world->global_cfm); |
|
524 dSetValue (lo,m,-dInfinity); |
|
525 dSetValue (hi,m, dInfinity); |
|
526 for (i=0; i<m; i++) findex[i] = -1; |
|
527 |
|
528 // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint |
|
529 // data. also fill the c vector. |
|
530 |
|
531 ALLOCA(dReal,J,m*nskip*sizeof(dReal)); |
|
532 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
533 if (J == NULL) { |
|
534 UNALLOCA(findex); |
|
535 UNALLOCA(hi); |
|
536 UNALLOCA(lo); |
|
537 UNALLOCA(cfm); |
|
538 UNALLOCA(c); |
|
539 UNALLOCA(vnew); |
|
540 UNALLOCA(v); |
|
541 UNALLOCA(fe); |
|
542 UNALLOCA(invM); |
|
543 UNALLOCA(ofs); |
|
544 UNALLOCA(info); |
|
545 UNALLOCA(invI); |
|
546 UNALLOCA(I); |
|
547 UNALLOCA(joint); |
|
548 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
549 return; |
|
550 } |
|
551 #endif |
|
552 dSetZero (J,m*nskip); |
|
553 dxJoint::Info2 Jinfo; |
|
554 Jinfo.rowskip = nskip; |
|
555 Jinfo.fps = dRecip(stepsize); |
|
556 Jinfo.erp = world->global_erp; |
|
557 for (i=0; i<nj; i++) { |
|
558 Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag; |
|
559 Jinfo.J1a = Jinfo.J1l + 3; |
|
560 if (joint[i]->node[1].body) { |
|
561 Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; |
|
562 Jinfo.J2a = Jinfo.J2l + 3; |
|
563 } |
|
564 else { |
|
565 Jinfo.J2l = 0; |
|
566 Jinfo.J2a = 0; |
|
567 } |
|
568 Jinfo.c = c + ofs[i]; |
|
569 Jinfo.cfm = cfm + ofs[i]; |
|
570 Jinfo.lo = lo + ofs[i]; |
|
571 Jinfo.hi = hi + ofs[i]; |
|
572 Jinfo.findex = findex + ofs[i]; |
|
573 joint[i]->vtable->getInfo2 (joint[i],&Jinfo); |
|
574 // adjust returned findex values for global index numbering |
|
575 for (j=0; j<info[i].m; j++) { |
|
576 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i]; |
|
577 } |
|
578 } |
|
579 |
|
580 // compute A = J*invM*J' |
|
581 |
|
582 ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal)); |
|
583 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
584 if (JinvM == NULL) { |
|
585 UNALLOCA(J); |
|
586 UNALLOCA(findex); |
|
587 UNALLOCA(hi); |
|
588 UNALLOCA(lo); |
|
589 UNALLOCA(cfm); |
|
590 UNALLOCA(c); |
|
591 UNALLOCA(vnew); |
|
592 UNALLOCA(v); |
|
593 UNALLOCA(fe); |
|
594 UNALLOCA(invM); |
|
595 UNALLOCA(ofs); |
|
596 UNALLOCA(info); |
|
597 UNALLOCA(invI); |
|
598 UNALLOCA(I); |
|
599 UNALLOCA(joint); |
|
600 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
601 return; |
|
602 } |
|
603 #endif |
|
604 //dSetZero (JinvM,m*nskip); |
|
605 dMultiply0 (JinvM,J,invM,m,n6,n6); |
|
606 int mskip = dPAD(m); |
|
607 ALLOCA(dReal,A,m*mskip*sizeof(dReal)); |
|
608 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
609 if (A == NULL) { |
|
610 UNALLOCA(JinvM); |
|
611 UNALLOCA(J); |
|
612 UNALLOCA(findex); |
|
613 UNALLOCA(hi); |
|
614 UNALLOCA(lo); |
|
615 UNALLOCA(cfm); |
|
616 UNALLOCA(c); |
|
617 UNALLOCA(vnew); |
|
618 UNALLOCA(v); |
|
619 UNALLOCA(fe); |
|
620 UNALLOCA(invM); |
|
621 UNALLOCA(ofs); |
|
622 UNALLOCA(info); |
|
623 UNALLOCA(invI); |
|
624 UNALLOCA(I); |
|
625 UNALLOCA(joint); |
|
626 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
627 return; |
|
628 } |
|
629 #endif |
|
630 //dSetZero (A,m*mskip); |
|
631 dMultiply2 (A,JinvM,J,m,n6,m); |
|
632 |
|
633 // add cfm to the diagonal of A |
|
634 for (i=0; i<m; i++) A[i*mskip+i] += dMUL(cfm[i],Jinfo.fps); |
|
635 |
|
636 // compute `rhs', the right hand side of the equation J*a=c |
|
637 ALLOCA(dReal,tmp1,n6*sizeof(dReal)); |
|
638 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
639 if (tmp1 == NULL) { |
|
640 UNALLOCA(A); |
|
641 UNALLOCA(JinvM); |
|
642 UNALLOCA(J); |
|
643 UNALLOCA(findex); |
|
644 UNALLOCA(hi); |
|
645 UNALLOCA(lo); |
|
646 UNALLOCA(cfm); |
|
647 UNALLOCA(c); |
|
648 UNALLOCA(vnew); |
|
649 UNALLOCA(v); |
|
650 UNALLOCA(fe); |
|
651 UNALLOCA(invM); |
|
652 UNALLOCA(ofs); |
|
653 UNALLOCA(info); |
|
654 UNALLOCA(invI); |
|
655 UNALLOCA(I); |
|
656 UNALLOCA(joint); |
|
657 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
658 return; |
|
659 } |
|
660 #endif |
|
661 //dSetZero (tmp1,n6); |
|
662 dMultiply0 (tmp1,invM,fe,n6,n6,1); |
|
663 for (i=0; i<n6; i++) tmp1[i] += dDIV(v[i],stepsize); |
|
664 ALLOCA(dReal,rhs,m*sizeof(dReal)); |
|
665 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
666 if (rhs == NULL) { |
|
667 UNALLOCA(tmp1); |
|
668 UNALLOCA(A); |
|
669 UNALLOCA(JinvM); |
|
670 UNALLOCA(J); |
|
671 UNALLOCA(findex); |
|
672 UNALLOCA(hi); |
|
673 UNALLOCA(lo); |
|
674 UNALLOCA(cfm); |
|
675 UNALLOCA(c); |
|
676 UNALLOCA(vnew); |
|
677 UNALLOCA(v); |
|
678 UNALLOCA(fe); |
|
679 UNALLOCA(invM); |
|
680 UNALLOCA(ofs); |
|
681 UNALLOCA(info); |
|
682 UNALLOCA(invI); |
|
683 UNALLOCA(I); |
|
684 UNALLOCA(joint); |
|
685 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
686 return; |
|
687 } |
|
688 #endif |
|
689 //dSetZero (rhs,m); |
|
690 dMultiply0 (rhs,J,tmp1,m,n6,1); |
|
691 for (i=0; i<m; i++) rhs[i] = dDIV(c[i],stepsize) - rhs[i]; |
|
692 |
|
693 |
|
694 |
|
695 |
|
696 #ifndef DIRECT_CHOLESKY |
|
697 // solve the LCP problem and get lambda. |
|
698 // this will destroy A but that's okay |
|
699 ALLOCA(dReal,lambda,m*sizeof(dReal)); |
|
700 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
701 if (lambda == NULL) { |
|
702 UNALLOCA(rhs); |
|
703 UNALLOCA(tmp1); |
|
704 UNALLOCA(A); |
|
705 UNALLOCA(JinvM); |
|
706 UNALLOCA(J); |
|
707 UNALLOCA(findex); |
|
708 UNALLOCA(hi); |
|
709 UNALLOCA(lo); |
|
710 UNALLOCA(cfm); |
|
711 UNALLOCA(c); |
|
712 UNALLOCA(vnew); |
|
713 UNALLOCA(v); |
|
714 UNALLOCA(fe); |
|
715 UNALLOCA(invM); |
|
716 UNALLOCA(ofs); |
|
717 UNALLOCA(info); |
|
718 UNALLOCA(invI); |
|
719 UNALLOCA(I); |
|
720 UNALLOCA(joint); |
|
721 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
722 return; |
|
723 } |
|
724 #endif |
|
725 ALLOCA(dReal,residual,m*sizeof(dReal)); |
|
726 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
727 if (residual == NULL) { |
|
728 UNALLOCA(lambda); |
|
729 UNALLOCA(rhs); |
|
730 UNALLOCA(tmp1); |
|
731 UNALLOCA(A); |
|
732 UNALLOCA(JinvM); |
|
733 UNALLOCA(J); |
|
734 UNALLOCA(findex); |
|
735 UNALLOCA(hi); |
|
736 UNALLOCA(lo); |
|
737 UNALLOCA(cfm); |
|
738 UNALLOCA(c); |
|
739 UNALLOCA(vnew); |
|
740 UNALLOCA(v); |
|
741 UNALLOCA(fe); |
|
742 UNALLOCA(invM); |
|
743 UNALLOCA(ofs); |
|
744 UNALLOCA(info); |
|
745 UNALLOCA(invI); |
|
746 UNALLOCA(I); |
|
747 UNALLOCA(joint); |
|
748 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
749 return; |
|
750 } |
|
751 #endif |
|
752 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex); |
|
753 UNALLOCA(residual); |
|
754 UNALLOCA(lambda); |
|
755 |
|
756 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
757 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) |
|
758 return; |
|
759 #endif |
|
760 |
|
761 |
|
762 #else |
|
763 |
|
764 // OLD WAY - direct factor and solve |
|
765 |
|
766 // factorize A (L*L'=A) |
|
767 |
|
768 ALLOCA(dReal,L,m*mskip*sizeof(dReal)); |
|
769 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
770 if (L == NULL) { |
|
771 UNALLOCA(rhs); |
|
772 UNALLOCA(tmp1); |
|
773 UNALLOCA(A); |
|
774 UNALLOCA(JinvM); |
|
775 UNALLOCA(J); |
|
776 UNALLOCA(findex); |
|
777 UNALLOCA(hi); |
|
778 UNALLOCA(lo); |
|
779 UNALLOCA(cfm); |
|
780 UNALLOCA(c); |
|
781 UNALLOCA(vnew); |
|
782 UNALLOCA(v); |
|
783 UNALLOCA(fe); |
|
784 UNALLOCA(invM); |
|
785 UNALLOCA(ofs); |
|
786 UNALLOCA(info); |
|
787 UNALLOCA(invI); |
|
788 UNALLOCA(I); |
|
789 UNALLOCA(joint); |
|
790 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
791 return; |
|
792 } |
|
793 #endif |
|
794 memcpy (L,A,m*mskip*sizeof(dReal)); |
|
795 |
|
796 // compute lambda |
|
797 |
|
798 ALLOCA(dReal,lambda,m*sizeof(dReal)); |
|
799 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
800 if (lambda == NULL) { |
|
801 UNALLOCA(L); |
|
802 UNALLOCA(rhs); |
|
803 UNALLOCA(tmp1); |
|
804 UNALLOCA(A); |
|
805 UNALLOCA(JinvM); |
|
806 UNALLOCA(J); |
|
807 UNALLOCA(findex); |
|
808 UNALLOCA(hi); |
|
809 UNALLOCA(lo); |
|
810 UNALLOCA(cfm); |
|
811 UNALLOCA(c); |
|
812 UNALLOCA(vnew); |
|
813 UNALLOCA(v); |
|
814 UNALLOCA(fe); |
|
815 UNALLOCA(invM); |
|
816 UNALLOCA(ofs); |
|
817 UNALLOCA(info); |
|
818 UNALLOCA(invI); |
|
819 UNALLOCA(I); |
|
820 UNALLOCA(joint); |
|
821 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
822 return; |
|
823 } |
|
824 #endif |
|
825 memcpy (lambda,rhs,m * sizeof(dReal)); |
|
826 dSolveCholesky (L,lambda,m); |
|
827 #endif |
|
828 |
|
829 |
|
830 // compute the velocity update `vnew' |
|
831 dMultiply1 (tmp1,J,lambda,n6,m,1); |
|
832 for (i=0; i<n6; i++) tmp1[i] += fe[i]; |
|
833 dMultiply0 (vnew,invM,tmp1,n6,n6,1); |
|
834 for (i=0; i<n6; i++) vnew[i] = v[i] + dMUL(stepsize,vnew[i]); |
|
835 |
|
836 |
|
837 UNALLOCA(c); |
|
838 UNALLOCA(cfm); |
|
839 UNALLOCA(lo); |
|
840 UNALLOCA(hi); |
|
841 UNALLOCA(findex); |
|
842 UNALLOCA(J); |
|
843 UNALLOCA(JinvM); |
|
844 UNALLOCA(A); |
|
845 UNALLOCA(tmp1); |
|
846 UNALLOCA(rhs); |
|
847 UNALLOCA(lambda); |
|
848 UNALLOCA(L); |
|
849 } |
|
850 else { |
|
851 // no constraints |
|
852 dMultiply0 (vnew,invM,fe,n6,n6,1); |
|
853 for (i=0; i<n6; i++) vnew[i] = v[i] + dMUL(stepsize,vnew[i]); |
|
854 } |
|
855 |
|
856 // apply the velocity update to the bodies |
|
857 for (i=0; i<nb; i++) { |
|
858 for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j]; |
|
859 for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; |
|
860 } |
|
861 |
|
862 // update the position and orientation from the new linear/angular velocity |
|
863 // (over the given timestep) |
|
864 |
|
865 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize); |
|
866 |
|
867 // zero all force accumulators |
|
868 for (i=0; i<nb; i++) { |
|
869 body[i]->facc[0] = 0; |
|
870 body[i]->facc[1] = 0; |
|
871 body[i]->facc[2] = 0; |
|
872 body[i]->facc[3] = 0; |
|
873 body[i]->tacc[0] = 0; |
|
874 body[i]->tacc[1] = 0; |
|
875 body[i]->tacc[2] = 0; |
|
876 body[i]->tacc[3] = 0; |
|
877 } |
|
878 |
|
879 |
|
880 UNALLOCA(joint); |
|
881 UNALLOCA(I); |
|
882 UNALLOCA(invI); |
|
883 UNALLOCA(info); |
|
884 UNALLOCA(ofs); |
|
885 UNALLOCA(invM); |
|
886 UNALLOCA(fe); |
|
887 UNALLOCA(v); |
|
888 UNALLOCA(vnew); |
|
889 } |
|
890 |
|
891 //**************************************************************************** |
|
892 // an optimized version of dInternalStepIsland1() |
|
893 |
|
894 void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, |
|
895 dxJoint * const *_joint, int nj, dReal stepsize) |
|
896 { |
|
897 int i,j,k; |
|
898 dReal stepsize1 = dRecip(stepsize); |
|
899 |
|
900 // number all bodies in the body list - set their tag values |
|
901 for (i=0; i<nb; i++) body[i]->tag = i; |
|
902 |
|
903 // make a local copy of the joint array, because we might want to modify it. |
|
904 // (the "dxJoint *const*" declaration says we're allowed to modify the joints |
|
905 // but not the joint array, because the caller might need it unchanged). |
|
906 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); |
|
907 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
908 if (joint == NULL) { |
|
909 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
910 return; |
|
911 } |
|
912 #endif |
|
913 memcpy (joint,_joint,nj * sizeof(dxJoint*)); |
|
914 |
|
915 // for all bodies, compute the inertia tensor and its inverse in the global |
|
916 // frame, and compute the rotational force and add it to the torque |
|
917 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. |
|
918 // @@@ check computation of rotational force. |
|
919 |
|
920 dReal *I = NULL; |
|
921 |
|
922 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); |
|
923 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
924 if (invI == NULL) { |
|
925 UNALLOCA(I); |
|
926 UNALLOCA(joint); |
|
927 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
928 return; |
|
929 } |
|
930 #endif |
|
931 |
|
932 //dSetZero (I,3*nb*4); |
|
933 //dSetZero (invI,3*nb*4); |
|
934 for (i=0; i<nb; i++) { |
|
935 dReal tmp[12]; |
|
936 |
|
937 // compute inverse inertia tensor in global frame |
|
938 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R); |
|
939 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); |
|
940 } |
|
941 |
|
942 // add the gravity force to all bodies |
|
943 for (i=0; i<nb; i++) { |
|
944 if ((body[i]->flags & dxBodyNoGravity)==0) { |
|
945 body[i]->facc[0] += dMUL(body[i]->mass.mass,world->gravity[0]); |
|
946 body[i]->facc[1] += dMUL(body[i]->mass.mass,world->gravity[1]); |
|
947 body[i]->facc[2] += dMUL(body[i]->mass.mass,world->gravity[2]); |
|
948 } |
|
949 } |
|
950 |
|
951 // get m = total constraint dimension, nub = number of unbounded variables. |
|
952 // create constraint offset array and number-of-rows array for all joints. |
|
953 // the constraints are re-ordered as follows: the purely unbounded |
|
954 // constraints, the mixed unbounded + LCP constraints, and last the purely |
|
955 // LCP constraints. this assists the LCP solver to put all unbounded |
|
956 // variables at the start for a quick factorization. |
|
957 // |
|
958 // joints with m=0 are inactive and are removed from the joints array |
|
959 // entirely, so that the code that follows does not consider them. |
|
960 // also number all active joints in the joint list (set their tag values). |
|
961 // inactive joints receive a tag value of -1. |
|
962 |
|
963 int m = 0; |
|
964 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); |
|
965 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
966 if (info == NULL) { |
|
967 UNALLOCA(invI); |
|
968 UNALLOCA(I); |
|
969 UNALLOCA(joint); |
|
970 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
971 return; |
|
972 } |
|
973 #endif |
|
974 ALLOCA(int,ofs,nj*sizeof(int)); |
|
975 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
976 if (ofs == NULL) { |
|
977 UNALLOCA(info); |
|
978 UNALLOCA(invI); |
|
979 UNALLOCA(I); |
|
980 UNALLOCA(joint); |
|
981 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
982 return; |
|
983 } |
|
984 #endif |
|
985 for (i=0, j=0; j<nj; j++) { // i=dest, j=src |
|
986 joint[j]->vtable->getInfo1 (joint[j],info+i); |
|
987 |
|
988 if (info[i].m > 0) { |
|
989 joint[i] = joint[j]; |
|
990 joint[i]->tag = i; |
|
991 i++; |
|
992 } |
|
993 else { |
|
994 joint[j]->tag = -1; |
|
995 } |
|
996 } |
|
997 nj = i; |
|
998 |
|
999 // the purely unbounded constraints |
|
1000 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) { |
|
1001 ofs[i] = m; |
|
1002 m += info[i].m; |
|
1003 } |
|
1004 int nub = m; |
|
1005 // the mixed unbounded + LCP constraints |
|
1006 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) { |
|
1007 ofs[i] = m; |
|
1008 m += info[i].m; |
|
1009 } |
|
1010 // the purely LCP constraints |
|
1011 for (i=0; i<nj; i++) if (info[i].nub == 0) { |
|
1012 ofs[i] = m; |
|
1013 m += info[i].m; |
|
1014 } |
|
1015 |
|
1016 // this will be set to the force due to the constraints |
|
1017 ALLOCA(dReal,cforce,nb*8*sizeof(dReal)); |
|
1018 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1019 if (cforce == NULL) { |
|
1020 UNALLOCA(ofs); |
|
1021 UNALLOCA(info); |
|
1022 UNALLOCA(invI); |
|
1023 UNALLOCA(I); |
|
1024 UNALLOCA(joint); |
|
1025 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1026 return; |
|
1027 } |
|
1028 #endif |
|
1029 dSetZero (cforce,nb*8); |
|
1030 |
|
1031 // if there are constraints, compute cforce |
|
1032 if (m > 0) { |
|
1033 // create a constraint equation right hand side vector `c', a constraint |
|
1034 // force mixing vector `cfm', and LCP low and high bound vectors, and an |
|
1035 // 'findex' vector. |
|
1036 ALLOCA(dReal,c,m*sizeof(dReal)); |
|
1037 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1038 if (c == NULL) { |
|
1039 UNALLOCA(cforce); |
|
1040 UNALLOCA(ofs); |
|
1041 UNALLOCA(info); |
|
1042 UNALLOCA(invI); |
|
1043 UNALLOCA(I); |
|
1044 UNALLOCA(joint); |
|
1045 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1046 return; |
|
1047 } |
|
1048 #endif |
|
1049 ALLOCA(dReal,cfm,m*sizeof(dReal)); |
|
1050 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1051 if (cfm == NULL) { |
|
1052 UNALLOCA(c); |
|
1053 UNALLOCA(cforce); |
|
1054 UNALLOCA(ofs); |
|
1055 UNALLOCA(info); |
|
1056 UNALLOCA(invI); |
|
1057 UNALLOCA(I); |
|
1058 UNALLOCA(joint); |
|
1059 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1060 return; |
|
1061 } |
|
1062 #endif |
|
1063 ALLOCA(dReal,lo,m*sizeof(dReal)); |
|
1064 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1065 if (lo == NULL) { |
|
1066 UNALLOCA(cfm); |
|
1067 UNALLOCA(c); |
|
1068 UNALLOCA(cforce); |
|
1069 UNALLOCA(ofs); |
|
1070 UNALLOCA(info); |
|
1071 UNALLOCA(invI); |
|
1072 UNALLOCA(I); |
|
1073 UNALLOCA(joint); |
|
1074 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1075 return; |
|
1076 } |
|
1077 #endif |
|
1078 ALLOCA(dReal,hi,m*sizeof(dReal)); |
|
1079 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1080 if (hi == NULL) { |
|
1081 UNALLOCA(lo); |
|
1082 UNALLOCA(cfm); |
|
1083 UNALLOCA(c); |
|
1084 UNALLOCA(cforce); |
|
1085 UNALLOCA(ofs); |
|
1086 UNALLOCA(info); |
|
1087 UNALLOCA(invI); |
|
1088 UNALLOCA(I); |
|
1089 UNALLOCA(joint); |
|
1090 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1091 return; |
|
1092 } |
|
1093 #endif |
|
1094 ALLOCA(int,findex,m*sizeof(int)); |
|
1095 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1096 if (findex == NULL) { |
|
1097 UNALLOCA(hi); |
|
1098 UNALLOCA(lo); |
|
1099 UNALLOCA(cfm); |
|
1100 UNALLOCA(c); |
|
1101 UNALLOCA(cforce); |
|
1102 UNALLOCA(ofs); |
|
1103 UNALLOCA(info); |
|
1104 UNALLOCA(invI); |
|
1105 UNALLOCA(I); |
|
1106 UNALLOCA(joint); |
|
1107 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1108 return; |
|
1109 } |
|
1110 #endif |
|
1111 dSetZero (c,m); |
|
1112 dSetValue (cfm,m,world->global_cfm); |
|
1113 dSetValue (lo,m,-dInfinity); |
|
1114 dSetValue (hi,m, dInfinity); |
|
1115 for (i=0; i<m; i++) findex[i] = -1; |
|
1116 |
|
1117 // get jacobian data from constraints. a (2*m)x8 matrix will be created |
|
1118 // to store the two jacobian blocks from each constraint. it has this |
|
1119 // format: |
|
1120 // |
|
1121 // l l l 0 a a a 0 \ . |
|
1122 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) |
|
1123 // l l l 0 a a a 0 / |
|
1124 // l l l 0 a a a 0 \ . |
|
1125 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) |
|
1126 // l l l 0 a a a 0 / |
|
1127 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) |
|
1128 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) |
|
1129 // etc... |
|
1130 // |
|
1131 // (lll) = linear jacobian data |
|
1132 // (aaa) = angular jacobian data |
|
1133 // |
|
1134 ALLOCA(dReal,J,2*m*8*sizeof(dReal)); |
|
1135 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1136 if (J == NULL) { |
|
1137 UNALLOCA(findex); |
|
1138 UNALLOCA(hi); |
|
1139 UNALLOCA(lo); |
|
1140 UNALLOCA(cfm); |
|
1141 UNALLOCA(c); |
|
1142 UNALLOCA(cforce); |
|
1143 UNALLOCA(ofs); |
|
1144 UNALLOCA(info); |
|
1145 UNALLOCA(invI); |
|
1146 UNALLOCA(I); |
|
1147 UNALLOCA(joint); |
|
1148 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1149 return; |
|
1150 } |
|
1151 #endif |
|
1152 dSetZero (J,2*m*8); |
|
1153 dxJoint::Info2 Jinfo; |
|
1154 Jinfo.rowskip = 8; |
|
1155 Jinfo.fps = stepsize1; |
|
1156 Jinfo.erp = world->global_erp; |
|
1157 for (i=0; i<nj; i++) { |
|
1158 Jinfo.J1l = J + 2*8*ofs[i]; |
|
1159 Jinfo.J1a = Jinfo.J1l + 4; |
|
1160 Jinfo.J2l = Jinfo.J1l + 8*info[i].m; |
|
1161 Jinfo.J2a = Jinfo.J2l + 4; |
|
1162 Jinfo.c = c + ofs[i]; |
|
1163 Jinfo.cfm = cfm + ofs[i]; |
|
1164 Jinfo.lo = lo + ofs[i]; |
|
1165 Jinfo.hi = hi + ofs[i]; |
|
1166 Jinfo.findex = findex + ofs[i]; |
|
1167 joint[i]->vtable->getInfo2 (joint[i],&Jinfo); |
|
1168 // adjust returned findex values for global index numbering |
|
1169 for (j=0; j<info[i].m; j++) { |
|
1170 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i]; |
|
1171 } |
|
1172 } |
|
1173 |
|
1174 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same |
|
1175 // format as J so we just go through the constraints in J multiplying by |
|
1176 // the appropriate scalars and matrices. |
|
1177 |
|
1178 ALLOCA(dReal,JinvM,2*m*8*sizeof(dReal)); |
|
1179 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1180 if (JinvM == NULL) { |
|
1181 UNALLOCA(J); |
|
1182 UNALLOCA(findex); |
|
1183 UNALLOCA(hi); |
|
1184 UNALLOCA(lo); |
|
1185 UNALLOCA(cfm); |
|
1186 UNALLOCA(c); |
|
1187 UNALLOCA(cforce); |
|
1188 UNALLOCA(ofs); |
|
1189 UNALLOCA(info); |
|
1190 UNALLOCA(invI); |
|
1191 UNALLOCA(I); |
|
1192 UNALLOCA(joint); |
|
1193 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1194 return; |
|
1195 } |
|
1196 #endif |
|
1197 dSetZero (JinvM,2*m*8); |
|
1198 for (i=0; i<nj; i++) { |
|
1199 int b = joint[i]->node[0].body->tag; |
|
1200 dReal body_invMass = body[b]->invMass; |
|
1201 dReal *body_invI = invI + b*12; |
|
1202 dReal *Jsrc = J + 2*8*ofs[i]; |
|
1203 dReal *Jdst = JinvM + 2*8*ofs[i]; |
|
1204 for (j=info[i].m-1; j>=0; j--) { |
|
1205 for (k=0; k<3; k++) Jdst[k] = dMUL(Jsrc[k],body_invMass); |
|
1206 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); |
|
1207 Jsrc += 8; |
|
1208 Jdst += 8; |
|
1209 } |
|
1210 if (joint[i]->node[1].body) { |
|
1211 b = joint[i]->node[1].body->tag; |
|
1212 body_invMass = body[b]->invMass; |
|
1213 body_invI = invI + b*12; |
|
1214 for (j=info[i].m-1; j>=0; j--) { |
|
1215 for (k=0; k<3; k++) Jdst[k] = dMUL(Jsrc[k],body_invMass); |
|
1216 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); |
|
1217 Jsrc += 8; |
|
1218 Jdst += 8; |
|
1219 } |
|
1220 } |
|
1221 } |
|
1222 |
|
1223 // now compute A = JinvM * J'. A's rows and columns are grouped by joint, |
|
1224 // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero |
|
1225 // if joints i and j have at least one body in common. this fact suggests |
|
1226 // the algorithm used to fill A: |
|
1227 // |
|
1228 // for b = all bodies |
|
1229 // n = number of joints attached to body b |
|
1230 // for i = 1..n |
|
1231 // for j = i+1..n |
|
1232 // ii = actual joint number for i |
|
1233 // jj = actual joint number for j |
|
1234 // // (ii,jj) will be set to all pairs of joints around body b |
|
1235 // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' |
|
1236 // |
|
1237 // this algorithm catches all pairs of joints that have at least one body |
|
1238 // in common. it does not compute the diagonal blocks of A however - |
|
1239 // another similar algorithm does that. |
|
1240 |
|
1241 int mskip = dPAD(m); |
|
1242 ALLOCA(dReal,A,m*mskip*sizeof(dReal)); |
|
1243 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1244 if (A == NULL) { |
|
1245 UNALLOCA(JinvM); |
|
1246 UNALLOCA(J); |
|
1247 UNALLOCA(findex); |
|
1248 UNALLOCA(hi); |
|
1249 UNALLOCA(lo); |
|
1250 UNALLOCA(cfm); |
|
1251 UNALLOCA(c); |
|
1252 UNALLOCA(cforce); |
|
1253 UNALLOCA(ofs); |
|
1254 UNALLOCA(info); |
|
1255 UNALLOCA(invI); |
|
1256 UNALLOCA(I); |
|
1257 UNALLOCA(joint); |
|
1258 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1259 return; |
|
1260 } |
|
1261 #endif |
|
1262 dSetZero (A,m*mskip); |
|
1263 for (i=0; i<nb; i++) { |
|
1264 for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) { |
|
1265 for (dxJointNode *n2=n1->next; n2; n2=n2->next) { |
|
1266 // get joint numbers and ensure ofs[j1] >= ofs[j2] |
|
1267 int j1 = n1->joint->tag; |
|
1268 int j2 = n2->joint->tag; |
|
1269 if (ofs[j1] < ofs[j2]) { |
|
1270 int tmp = j1; |
|
1271 j1 = j2; |
|
1272 j2 = tmp; |
|
1273 } |
|
1274 |
|
1275 // if either joint was tagged as -1 then it is an inactive (m=0) |
|
1276 // joint that should not be considered |
|
1277 if (j1==-1 || j2==-1) continue; |
|
1278 |
|
1279 // determine if body i is the 1st or 2nd body of joints j1 and j2 |
|
1280 int jb1 = (joint[j1]->node[1].body == body[i]); |
|
1281 int jb2 = (joint[j2]->node[1].body == body[i]); |
|
1282 // jb1/jb2 must be 0 for joints with only one body |
|
1283 |
|
1284 |
|
1285 // set block of A |
|
1286 MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], |
|
1287 JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, |
|
1288 J + 2*8*ofs[j2] + jb2*8*info[j2].m, |
|
1289 info[j1].m,info[j2].m, mskip); |
|
1290 } |
|
1291 } |
|
1292 } |
|
1293 // compute diagonal blocks of A |
|
1294 for (i=0; i<nj; i++) { |
|
1295 Multiply2_p8r (A + ofs[i]*(mskip+1), |
|
1296 JinvM + 2*8*ofs[i], |
|
1297 J + 2*8*ofs[i], |
|
1298 info[i].m,info[i].m, mskip); |
|
1299 if (joint[i]->node[1].body) { |
|
1300 MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), |
|
1301 JinvM + 2*8*ofs[i] + 8*info[i].m, |
|
1302 J + 2*8*ofs[i] + 8*info[i].m, |
|
1303 info[i].m,info[i].m, mskip); |
|
1304 } |
|
1305 } |
|
1306 |
|
1307 // add cfm to the diagonal of A |
|
1308 for (i=0; i<m; i++) A[i*mskip+i] += dMUL(cfm[i],stepsize1); |
|
1309 |
|
1310 // compute the right hand side `rhs' |
|
1311 ALLOCA(dReal,tmp1,nb*8*sizeof(dReal)); |
|
1312 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1313 if (tmp1 == NULL) { |
|
1314 UNALLOCA(A); |
|
1315 UNALLOCA(JinvM); |
|
1316 UNALLOCA(J); |
|
1317 UNALLOCA(findex); |
|
1318 UNALLOCA(hi); |
|
1319 UNALLOCA(lo); |
|
1320 UNALLOCA(cfm); |
|
1321 UNALLOCA(c); |
|
1322 UNALLOCA(cforce); |
|
1323 UNALLOCA(ofs); |
|
1324 UNALLOCA(info); |
|
1325 UNALLOCA(invI); |
|
1326 UNALLOCA(I); |
|
1327 UNALLOCA(joint); |
|
1328 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1329 return; |
|
1330 } |
|
1331 #endif |
|
1332 //dSetZero (tmp1,nb*8); |
|
1333 // put v/h + invM*fe into tmp1 |
|
1334 for (i=0; i<nb; i++) { |
|
1335 dReal body_invMass = body[i]->invMass; |
|
1336 dReal *body_invI = invI + i*12; |
|
1337 for (j=0; j<3; j++) tmp1[i*8+j] = dMUL(body[i]->facc[j],body_invMass) + |
|
1338 dMUL(body[i]->lvel[j],stepsize1); |
|
1339 dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); |
|
1340 for (j=0; j<3; j++) tmp1[i*8+4+j] += dMUL(body[i]->avel[j],stepsize1); |
|
1341 } |
|
1342 // put J*tmp1 into rhs |
|
1343 ALLOCA(dReal,rhs,m*sizeof(dReal)); |
|
1344 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1345 if (rhs == NULL) { |
|
1346 UNALLOCA(tmp1); |
|
1347 UNALLOCA(A); |
|
1348 UNALLOCA(JinvM); |
|
1349 UNALLOCA(J); |
|
1350 UNALLOCA(findex); |
|
1351 UNALLOCA(hi); |
|
1352 UNALLOCA(lo); |
|
1353 UNALLOCA(cfm); |
|
1354 UNALLOCA(c); |
|
1355 UNALLOCA(cforce); |
|
1356 UNALLOCA(ofs); |
|
1357 UNALLOCA(info); |
|
1358 UNALLOCA(invI); |
|
1359 UNALLOCA(I); |
|
1360 UNALLOCA(joint); |
|
1361 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1362 return; |
|
1363 } |
|
1364 #endif |
|
1365 //dSetZero (rhs,m); |
|
1366 for (i=0; i<nj; i++) { |
|
1367 dReal *JJ = J + 2*8*ofs[i]; |
|
1368 Multiply0_p81 (rhs+ofs[i],JJ, |
|
1369 tmp1 + 8*joint[i]->node[0].body->tag, info[i].m); |
|
1370 if (joint[i]->node[1].body) { |
|
1371 MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, |
|
1372 tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); |
|
1373 } |
|
1374 } |
|
1375 // complete rhs |
|
1376 for (i=0; i<m; i++) rhs[i] = dMUL(c[i],stepsize1) - rhs[i]; |
|
1377 |
|
1378 // solve the LCP problem and get lambda. |
|
1379 // this will destroy A but that's okay |
|
1380 ALLOCA(dReal,lambda,m*sizeof(dReal)); |
|
1381 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1382 if (lambda == NULL) { |
|
1383 UNALLOCA(rhs); |
|
1384 UNALLOCA(tmp1); |
|
1385 UNALLOCA(A); |
|
1386 UNALLOCA(JinvM); |
|
1387 UNALLOCA(J); |
|
1388 UNALLOCA(findex); |
|
1389 UNALLOCA(hi); |
|
1390 UNALLOCA(lo); |
|
1391 UNALLOCA(cfm); |
|
1392 UNALLOCA(c); |
|
1393 UNALLOCA(cforce); |
|
1394 UNALLOCA(ofs); |
|
1395 UNALLOCA(info); |
|
1396 UNALLOCA(invI); |
|
1397 UNALLOCA(I); |
|
1398 UNALLOCA(joint); |
|
1399 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1400 return; |
|
1401 } |
|
1402 #endif |
|
1403 ALLOCA(dReal,residual,m*sizeof(dReal)); |
|
1404 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1405 if (residual == NULL) { |
|
1406 UNALLOCA(lambda); |
|
1407 UNALLOCA(rhs); |
|
1408 UNALLOCA(tmp1); |
|
1409 UNALLOCA(A); |
|
1410 UNALLOCA(JinvM); |
|
1411 UNALLOCA(J); |
|
1412 UNALLOCA(findex); |
|
1413 UNALLOCA(hi); |
|
1414 UNALLOCA(lo); |
|
1415 UNALLOCA(cfm); |
|
1416 UNALLOCA(c); |
|
1417 UNALLOCA(cforce); |
|
1418 UNALLOCA(ofs); |
|
1419 UNALLOCA(info); |
|
1420 UNALLOCA(invI); |
|
1421 UNALLOCA(I); |
|
1422 UNALLOCA(joint); |
|
1423 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
|
1424 return; |
|
1425 } |
|
1426 #endif |
|
1427 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex); |
|
1428 |
|
1429 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1430 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) |
|
1431 return; |
|
1432 #endif |
|
1433 |
|
1434 |
|
1435 // OLD WAY - direct factor and solve |
|
1436 // |
|
1437 // // factorize A (L*L'=A) |
|
1438 //# ifdef TIMING |
|
1439 // dTimerNow ("factorize A"); |
|
1440 //# endif |
|
1441 // dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); |
|
1442 // memcpy (L,A,m*mskip*sizeof(dReal)); |
|
1443 //# ifdef FAST_FACTOR |
|
1444 // dFastFactorCholesky (L,m); // does not report non positive definiteness |
|
1445 //# else |
|
1446 // if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite"); |
|
1447 //# endif |
|
1448 // |
|
1449 // // compute lambda |
|
1450 //# ifdef TIMING |
|
1451 // dTimerNow ("compute lambda"); |
|
1452 //# endif |
|
1453 // dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal)); |
|
1454 // memcpy (lambda,rhs,m * sizeof(dReal)); |
|
1455 // dSolveCholesky (L,lambda,m); |
|
1456 |
|
1457 |
|
1458 // compute the constraint force `cforce' |
|
1459 // compute cforce = J'*lambda |
|
1460 for (i=0; i<nj; i++) { |
|
1461 dReal *JJ = J + 2*8*ofs[i]; |
|
1462 dxBody* b1 = joint[i]->node[0].body; |
|
1463 dxBody* b2 = joint[i]->node[1].body; |
|
1464 dJointFeedback *fb = joint[i]->feedback; |
|
1465 |
|
1466 if (fb) { |
|
1467 // the user has requested feedback on the amount of force that this |
|
1468 // joint is applying to the bodies. we use a slightly slower |
|
1469 // computation that splits out the force components and puts them |
|
1470 // in the feedback structure. |
|
1471 dReal data1[8],data2[8]; |
|
1472 Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); |
|
1473 dReal *cf1 = cforce + 8*b1->tag; |
|
1474 cf1[0] += (fb->f1[0] = data1[0]); |
|
1475 cf1[1] += (fb->f1[1] = data1[1]); |
|
1476 cf1[2] += (fb->f1[2] = data1[2]); |
|
1477 cf1[4] += (fb->t1[0] = data1[4]); |
|
1478 cf1[5] += (fb->t1[1] = data1[5]); |
|
1479 cf1[6] += (fb->t1[2] = data1[6]); |
|
1480 if (b2){ |
|
1481 Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); |
|
1482 dReal *cf2 = cforce + 8*b2->tag; |
|
1483 cf2[0] += (fb->f2[0] = data2[0]); |
|
1484 cf2[1] += (fb->f2[1] = data2[1]); |
|
1485 cf2[2] += (fb->f2[2] = data2[2]); |
|
1486 cf2[4] += (fb->t2[0] = data2[4]); |
|
1487 cf2[5] += (fb->t2[1] = data2[5]); |
|
1488 cf2[6] += (fb->t2[2] = data2[6]); |
|
1489 } |
|
1490 } |
|
1491 else { |
|
1492 // no feedback is required, let's compute cforce the faster way |
|
1493 MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); |
|
1494 if (b2) { |
|
1495 MultiplyAdd1_8q1 (cforce + 8*b2->tag, |
|
1496 JJ + 8*info[i].m, lambda+ofs[i], info[i].m); |
|
1497 } |
|
1498 } |
|
1499 } |
|
1500 UNALLOCA(c); |
|
1501 UNALLOCA(cfm); |
|
1502 UNALLOCA(lo); |
|
1503 UNALLOCA(hi); |
|
1504 UNALLOCA(findex); |
|
1505 UNALLOCA(J); |
|
1506 UNALLOCA(JinvM); |
|
1507 UNALLOCA(A); |
|
1508 UNALLOCA(tmp1); |
|
1509 UNALLOCA(rhs); |
|
1510 UNALLOCA(lambda); |
|
1511 UNALLOCA(residual); |
|
1512 } |
|
1513 |
|
1514 // compute the velocity update |
|
1515 // add fe to cforce |
|
1516 for (i=0; i<nb; i++) { |
|
1517 for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j]; |
|
1518 for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; |
|
1519 } |
|
1520 // multiply cforce by stepsize |
|
1521 for (i=0; i < nb*8; i++) cforce[i] = dMUL(cforce[i],stepsize); |
|
1522 // add invM * cforce to the body velocity |
|
1523 for (i=0; i<nb; i++) { |
|
1524 dReal body_invMass = body[i]->invMass; |
|
1525 dReal *body_invI = invI + i*12; |
|
1526 for (j=0; j<3; j++) body[i]->lvel[j] += dMUL(body_invMass,cforce[i*8+j]); |
|
1527 dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); |
|
1528 } |
|
1529 |
|
1530 // update the position and orientation from the new linear/angular velocity |
|
1531 // (over the given timestep) |
|
1532 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize); |
|
1533 |
|
1534 // zero all force accumulators |
|
1535 for (i=0; i<nb; i++) { |
|
1536 body[i]->facc[0] = 0; |
|
1537 body[i]->facc[1] = 0; |
|
1538 body[i]->facc[2] = 0; |
|
1539 body[i]->facc[3] = 0; |
|
1540 body[i]->tacc[0] = 0; |
|
1541 body[i]->tacc[1] = 0; |
|
1542 body[i]->tacc[2] = 0; |
|
1543 body[i]->tacc[3] = 0; |
|
1544 } |
|
1545 |
|
1546 UNALLOCA(joint); |
|
1547 UNALLOCA(I); |
|
1548 UNALLOCA(invI); |
|
1549 UNALLOCA(info); |
|
1550 UNALLOCA(ofs); |
|
1551 UNALLOCA(cforce); |
|
1552 } |
|
1553 |
|
1554 //**************************************************************************** |
|
1555 |
|
1556 void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, |
|
1557 dxJoint * const *joint, int nj, dReal stepsize) |
|
1558 { |
|
1559 |
|
1560 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1561 dMemoryFlag = d_MEMORY_OK; |
|
1562 #endif |
|
1563 |
|
1564 #ifndef COMPARE_METHODS |
|
1565 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); |
|
1566 |
|
1567 #ifdef dUSE_MALLOC_FOR_ALLOCA |
|
1568 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { |
|
1569 REPORT_OUT_OF_MEMORY; |
|
1570 return; |
|
1571 } |
|
1572 #endif |
|
1573 |
|
1574 #endif |
|
1575 |
|
1576 } |
|
1577 |
|
1578 |