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