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 | |
---|
39 | int 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 | |
---|
80 | void 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 | |
---|
89 | void 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 | |
---|
113 | void 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 | |
---|
120 | void 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 | |
---|
136 | void 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 | |
---|
160 | void 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 | |
---|
168 | void 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 | |
---|
175 | void 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 | |
---|
196 | void 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 | |
---|
203 | void 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 | */ |
---|
231 | void 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 | |
---|
416 | void 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 | |
---|
429 | void 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 | |
---|
442 | void 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 | |
---|
486 | void 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 | |
---|
521 | void 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 | } |
---|