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 <ode/odeconfig.h>
24 #include "config.h"
25 #include <ode/mass.h>
26 #include <ode/odemath.h>
27 #include <ode/matrix.h>
28
29 // Local dependencies
30 #include "collision_kernel.h"
31
32 #if dTRIMESH_ENABLED
33 #include "collision_trimesh_internal.h"
34 #endif // dTRIMESH_ENABLED
35
36 #define SQR(x) ((x)*(x)) //!< Returns x square
37 #define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
38
39 #define _I(i,j) I[(i)*4+(j)]
40
41
42 // return 1 if ok, 0 if bad
43
dMassCheck(const dMass * m)44 int dMassCheck (const dMass *m)
45 {
46 int i;
47
48 if (m->mass <= 0) {
49 dDEBUGMSG ("mass must be > 0");
50 return 0;
51 }
52 if (!dIsPositiveDefinite (m->I,3)) {
53 dDEBUGMSG ("inertia must be positive definite");
54 return 0;
55 }
56
57 // verify that the center of mass position is consistent with the mass
58 // and inertia matrix. this is done by checking that the inertia around
59 // the center of mass is also positive definite. from the comment in
60 // dMassTranslate(), if the body is translated so that its center of mass
61 // is at the point of reference, then the new inertia is:
62 // I + mass*crossmat(c)^2
63 // note that requiring this to be positive definite is exactly equivalent
64 // to requiring that the spatial inertia matrix
65 // [ mass*eye(3,3) M*crossmat(c)^T ]
66 // [ M*crossmat(c) I ]
67 // is positive definite, given that I is PD and mass>0. see the theorem
68 // about partitioned PD matrices for proof.
69
70 dMatrix3 I2,chat;
71 dSetZero (chat,12);
72 dCROSSMAT (chat,m->c,4,+,-);
73 dMULTIPLY0_333 (I2,chat,chat);
74 for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i];
75 for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i];
76 for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i];
77 if (!dIsPositiveDefinite (I2,3)) {
78 dDEBUGMSG ("center of mass inconsistent with mass parameters");
79 return 0;
80 }
81 return 1;
82 }
83
84
dMassSetZero(dMass * m)85 void dMassSetZero (dMass *m)
86 {
87 dAASSERT (m);
88 m->mass = REAL(0.0);
89 dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
90 dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
91 }
92
93
dMassSetParameters(dMass * m,dReal themass,dReal cgx,dReal cgy,dReal cgz,dReal I11,dReal I22,dReal I33,dReal I12,dReal I13,dReal I23)94 void dMassSetParameters (dMass *m, dReal themass,
95 dReal cgx, dReal cgy, dReal cgz,
96 dReal I11, dReal I22, dReal I33,
97 dReal I12, dReal I13, dReal I23)
98 {
99 dAASSERT (m);
100 dMassSetZero (m);
101 m->mass = themass;
102 m->c[0] = cgx;
103 m->c[1] = cgy;
104 m->c[2] = cgz;
105 m->_I(0,0) = I11;
106 m->_I(1,1) = I22;
107 m->_I(2,2) = I33;
108 m->_I(0,1) = I12;
109 m->_I(0,2) = I13;
110 m->_I(1,2) = I23;
111 m->_I(1,0) = I12;
112 m->_I(2,0) = I13;
113 m->_I(2,1) = I23;
114 dMassCheck (m);
115 }
116
117
dMassSetSphere(dMass * m,dReal density,dReal radius)118 void dMassSetSphere (dMass *m, dReal density, dReal radius)
119 {
120 dMassSetSphereTotal (m, (dReal) ((REAL(4.0)/REAL(3.0)) * M_PI *
121 radius*radius*radius * density), radius);
122 }
123
124
dMassSetSphereTotal(dMass * m,dReal total_mass,dReal radius)125 void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius)
126 {
127 dAASSERT (m);
128 dMassSetZero (m);
129 m->mass = total_mass;
130 dReal II = REAL(0.4) * total_mass * radius*radius;
131 m->_I(0,0) = II;
132 m->_I(1,1) = II;
133 m->_I(2,2) = II;
134
135 # ifndef dNODEBUG
136 dMassCheck (m);
137 # endif
138 }
139
140
dMassSetCapsule(dMass * m,dReal density,int direction,dReal radius,dReal length)141 void dMassSetCapsule (dMass *m, dReal density, int direction,
142 dReal radius, dReal length)
143 {
144 dReal M1,M2,Ia,Ib;
145 dAASSERT (m);
146 dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
147 dMassSetZero (m);
148 M1 = (dReal) (M_PI*radius*radius*length*density); // cylinder mass
149 M2 = (dReal) ((REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density); // total cap mass
150 m->mass = M1+M2;
151 Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) +
152 M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length);
153 Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius;
154 m->_I(0,0) = Ia;
155 m->_I(1,1) = Ia;
156 m->_I(2,2) = Ia;
157 m->_I(direction-1,direction-1) = Ib;
158
159 # ifndef dNODEBUG
160 dMassCheck (m);
161 # endif
162 }
163
164
dMassSetCapsuleTotal(dMass * m,dReal total_mass,int direction,dReal a,dReal b)165 void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction,
166 dReal a, dReal b)
167 {
168 dMassSetCapsule (m, 1.0, direction, a, b);
169 dMassAdjust (m, total_mass);
170 }
171
172
dMassSetCylinder(dMass * m,dReal density,int direction,dReal radius,dReal length)173 void dMassSetCylinder (dMass *m, dReal density, int direction,
174 dReal radius, dReal length)
175 {
176 dMassSetCylinderTotal (m, (dReal) (M_PI*radius*radius*length*density),
177 direction, radius, length);
178 }
179
dMassSetCylinderTotal(dMass * m,dReal total_mass,int direction,dReal radius,dReal length)180 void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
181 dReal radius, dReal length)
182 {
183 dReal r2,I;
184 dAASSERT (m);
185 dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
186 dMassSetZero (m);
187 r2 = radius*radius;
188 m->mass = total_mass;
189 I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
190 m->_I(0,0) = I;
191 m->_I(1,1) = I;
192 m->_I(2,2) = I;
193 m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
194
195 # ifndef dNODEBUG
196 dMassCheck (m);
197 # endif
198 }
199
200
dMassSetBox(dMass * m,dReal density,dReal lx,dReal ly,dReal lz)201 void dMassSetBox (dMass *m, dReal density,
202 dReal lx, dReal ly, dReal lz)
203 {
204 dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
205 }
206
207
dMassSetBoxTotal(dMass * m,dReal total_mass,dReal lx,dReal ly,dReal lz)208 void dMassSetBoxTotal (dMass *m, dReal total_mass,
209 dReal lx, dReal ly, dReal lz)
210 {
211 dAASSERT (m);
212 dMassSetZero (m);
213 m->mass = total_mass;
214 m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz);
215 m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz);
216 m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly);
217
218 # ifndef dNODEBUG
219 dMassCheck (m);
220 # endif
221 }
222
223
224
225
226
227
228 /*
229 * dMassSetTrimesh, implementation by Gero Mueller.
230 * Based on Brian Mirtich, "Fast and Accurate Computation of
231 * Polyhedral Mass Properties," journal of graphics tools, volume 1,
232 * number 2, 1996.
233 */
dMassSetTrimesh(dMass * m,dReal density,dGeomID g)234 void dMassSetTrimesh( dMass *m, dReal density, dGeomID g )
235 {
236 dAASSERT (m);
237 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
238
239 dMassSetZero (m);
240
241 #if dTRIMESH_ENABLED
242
243 dxTriMesh *TriMesh = (dxTriMesh *)g;
244 unsigned int triangles = FetchTriangleCount( TriMesh );
245
246 dReal nx, ny, nz;
247 unsigned int i, A, B, C;
248 // face integrals
249 dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
250
251 // projection integrals
252 dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
253
254 dReal T0 = 0;
255 dReal T1[3] = {0., 0., 0.};
256 dReal T2[3] = {0., 0., 0.};
257 dReal TP[3] = {0., 0., 0.};
258
259 for( i = 0; i < triangles; i++ )
260 {
261 dVector3 v[3];
262 FetchTransformedTriangle( TriMesh, i, v);
263
264 dVector3 n, a, b;
265 dOP( a, -, v[1], v[0] );
266 dOP( b, -, v[2], v[0] );
267 dCROSS( n, =, b, a );
268 nx = fabs(n[0]);
269 ny = fabs(n[1]);
270 nz = fabs(n[2]);
271
272 if( nx > ny && nx > nz )
273 C = 0;
274 else
275 C = (ny > nz) ? 1 : 2;
276
277 // Even though all triangles might be initially valid,
278 // a triangle may degenerate into a segment after applying
279 // space transformation.
280 if (n[C] != REAL(0.0))
281 {
282 A = (C + 1) % 3;
283 B = (A + 1) % 3;
284
285 // calculate face integrals
286 {
287 dReal w;
288 dReal k1, k2, k3, k4;
289
290 //compProjectionIntegrals(f);
291 {
292 dReal a0=0, a1=0, da;
293 dReal b0=0, b1=0, db;
294 dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
295 dReal a1_2, a1_3, b1_2, b1_3;
296 dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
297 dReal Cab, Kab, Caab, Kaab, Cabb, Kabb;
298
299 P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
300
301 for( int j = 0; j < 3; j++)
302 {
303 switch(j)
304 {
305 case 0:
306 a0 = v[0][A];
307 b0 = v[0][B];
308 a1 = v[1][A];
309 b1 = v[1][B];
310 break;
311 case 1:
312 a0 = v[1][A];
313 b0 = v[1][B];
314 a1 = v[2][A];
315 b1 = v[2][B];
316 break;
317 case 2:
318 a0 = v[2][A];
319 b0 = v[2][B];
320 a1 = v[0][A];
321 b1 = v[0][B];
322 break;
323 }
324 da = a1 - a0;
325 db = b1 - b0;
326 a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
327 b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
328 a1_2 = a1 * a1; a1_3 = a1_2 * a1;
329 b1_2 = b1 * b1; b1_3 = b1_2 * b1;
330
331 C1 = a1 + a0;
332 Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
333 Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
334 Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
335 Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
336 Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
337 Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
338
339 P1 += db*C1;
340 Pa += db*Ca;
341 Paa += db*Caa;
342 Paaa += db*Caaa;
343 Pb += da*Cb;
344 Pbb += da*Cbb;
345 Pbbb += da*Cbbb;
346 Pab += db*(b1*Cab + b0*Kab);
347 Paab += db*(b1*Caab + b0*Kaab);
348 Pabb += da*(a1*Cabb + a0*Kabb);
349 }
350
351 P1 /= 2.0;
352 Pa /= 6.0;
353 Paa /= 12.0;
354 Paaa /= 20.0;
355 Pb /= -6.0;
356 Pbb /= -12.0;
357 Pbbb /= -20.0;
358 Pab /= 24.0;
359 Paab /= 60.0;
360 Pabb /= -60.0;
361 }
362
363 w = - dDOT(n, v[0]);
364
365 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
366
367 Fa = k1 * Pa;
368 Fb = k1 * Pb;
369 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
370
371 Faa = k1 * Paa;
372 Fbb = k1 * Pbb;
373 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb +
374 w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
375
376 Faaa = k1 * Paaa;
377 Fbbb = k1 * Pbbb;
378 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
379 + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
380 + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
381 + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
382
383 Faab = k1 * Paab;
384 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
385 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
386 + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
387 }
388
389
390 T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
391
392 T1[A] += n[A] * Faa;
393 T1[B] += n[B] * Fbb;
394 T1[C] += n[C] * Fcc;
395 T2[A] += n[A] * Faaa;
396 T2[B] += n[B] * Fbbb;
397 T2[C] += n[C] * Fccc;
398 TP[A] += n[A] * Faab;
399 TP[B] += n[B] * Fbbc;
400 TP[C] += n[C] * Fcca;
401 }
402 }
403
404 T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
405 T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
406 TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
407
408 m->mass = density * T0;
409 m->_I(0,0) = density * (T2[1] + T2[2]);
410 m->_I(1,1) = density * (T2[2] + T2[0]);
411 m->_I(2,2) = density * (T2[0] + T2[1]);
412 m->_I(0,1) = - density * TP[0];
413 m->_I(1,0) = - density * TP[0];
414 m->_I(2,1) = - density * TP[1];
415 m->_I(1,2) = - density * TP[1];
416 m->_I(2,0) = - density * TP[2];
417 m->_I(0,2) = - density * TP[2];
418
419 // Added to address SF bug 1729095
420 dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 );
421
422 # ifndef dNODEBUG
423 dMassCheck (m);
424 # endif
425
426 #endif // dTRIMESH_ENABLED
427 }
428
429
dMassSetTrimeshTotal(dMass * m,dReal total_mass,dGeomID g)430 void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g)
431 {
432 dAASSERT( m );
433 dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" );
434 dMassSetTrimesh( m, 1.0, g );
435 dMassAdjust( m, total_mass );
436 }
437
438
439
440
dMassAdjust(dMass * m,dReal newmass)441 void dMassAdjust (dMass *m, dReal newmass)
442 {
443 dAASSERT (m);
444 dReal scale = newmass / m->mass;
445 m->mass = newmass;
446 for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
447
448 # ifndef dNODEBUG
449 dMassCheck (m);
450 # endif
451 }
452
453
dMassTranslate(dMass * m,dReal x,dReal y,dReal z)454 void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
455 {
456 // if the body is translated by `a' relative to its point of reference,
457 // the new inertia about the point of reference is:
458 //
459 // I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
460 //
461 // where c is the existing center of mass and I is the old inertia.
462
463 int i,j;
464 dMatrix3 ahat,chat,t1,t2;
465 dReal a[3];
466
467 dAASSERT (m);
468
469 // adjust inertia matrix
470 dSetZero (chat,12);
471 dCROSSMAT (chat,m->c,4,+,-);
472 a[0] = x + m->c[0];
473 a[1] = y + m->c[1];
474 a[2] = z + m->c[2];
475 dSetZero (ahat,12);
476 dCROSSMAT (ahat,a,4,+,-);
477 dMULTIPLY0_333 (t1,ahat,ahat);
478 dMULTIPLY0_333 (t2,chat,chat);
479 for (i=0; i<3; i++) for (j=0; j<3; j++)
480 m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
481
482 // ensure perfect symmetry
483 m->_I(1,0) = m->_I(0,1);
484 m->_I(2,0) = m->_I(0,2);
485 m->_I(2,1) = m->_I(1,2);
486
487 // adjust center of mass
488 m->c[0] += x;
489 m->c[1] += y;
490 m->c[2] += z;
491
492 # ifndef dNODEBUG
493 dMassCheck (m);
494 # endif
495 }
496
497
dMassRotate(dMass * m,const dMatrix3 R)498 void dMassRotate (dMass *m, const dMatrix3 R)
499 {
500 // if the body is rotated by `R' relative to its point of reference,
501 // the new inertia about the point of reference is:
502 //
503 // R * I * R'
504 //
505 // where I is the old inertia.
506
507 dMatrix3 t1;
508 dReal t2[3];
509
510 dAASSERT (m);
511
512 // rotate inertia matrix
513 dMULTIPLY2_333 (t1,m->I,R);
514 dMULTIPLY0_333 (m->I,R,t1);
515
516 // ensure perfect symmetry
517 m->_I(1,0) = m->_I(0,1);
518 m->_I(2,0) = m->_I(0,2);
519 m->_I(2,1) = m->_I(1,2);
520
521 // rotate center of mass
522 dMULTIPLY0_331 (t2,R,m->c);
523 m->c[0] = t2[0];
524 m->c[1] = t2[1];
525 m->c[2] = t2[2];
526
527 # ifndef dNODEBUG
528 dMassCheck (m);
529 # endif
530 }
531
532
dMassAdd(dMass * a,const dMass * b)533 void dMassAdd (dMass *a, const dMass *b)
534 {
535 int i;
536 dAASSERT (a && b);
537 dReal denom = dRecip (a->mass + b->mass);
538 for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
539 a->mass += b->mass;
540 for (i=0; i<12; i++) a->I[i] += b->I[i];
541 }
542
543
544 // Backwards compatible API
dMassSetCappedCylinder(dMass * a,dReal b,int c,dReal d,dReal e)545 void dMassSetCappedCylinder(dMass *a, dReal b, int c, dReal d, dReal e)
546 {
547 dMassSetCapsule(a,b,c,d,e);
548 }
549
dMassSetCappedCylinderTotal(dMass * a,dReal b,int c,dReal d,dReal e)550 void dMassSetCappedCylinderTotal(dMass *a, dReal b, int c, dReal d, dReal e)
551 {
552 dMassSetCapsuleTotal(a,b,c,d,e);
553 }
554
555