1 /* ----------------------------------------------------------------------
2 This is the
3
4 ██╗ ██╗ ██████╗ ██████╗ ██████╗ ██╗ ██╗████████╗███████╗
5 ██║ ██║██╔════╝ ██╔════╝ ██╔════╝ ██║ ██║╚══██╔══╝██╔════╝
6 ██║ ██║██║ ███╗██║ ███╗██║ ███╗███████║ ██║ ███████╗
7 ██║ ██║██║ ██║██║ ██║██║ ██║██╔══██║ ██║ ╚════██║
8 ███████╗██║╚██████╔╝╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████║
9 ╚══════╝╚═╝ ╚═════╝ ╚═════╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝®
10
11 DEM simulation engine, released by
12 DCS Computing Gmbh, Linz, Austria
13 http://www.dcs-computing.com, office@dcs-computing.com
14
15 LIGGGHTS® is part of CFDEM®project:
16 http://www.liggghts.com | http://www.cfdem.com
17
18 Core developer and main author:
19 Christoph Kloss, christoph.kloss@dcs-computing.com
20
21 LIGGGHTS® is open-source, distributed under the terms of the GNU Public
22 License, version 2 or later. It is distributed in the hope that it will
23 be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
24 of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. You should have
25 received a copy of the GNU General Public License along with LIGGGHTS®.
26 If not, see http://www.gnu.org/licenses . See also top-level README
27 and LICENSE files.
28
29 LIGGGHTS® and CFDEM® are registered trade marks of DCS Computing GmbH,
30 the producer of the LIGGGHTS® software and the CFDEM®coupling software
31 See http://www.cfdem.com/terms-trademark-policy for details.
32
33 -------------------------------------------------------------------------
34 Contributing author and copyright for this file:
35 This file is from LAMMPS
36 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
37 http://lammps.sandia.gov, Sandia National Laboratories
38 Steve Plimpton, sjplimp@sandia.gov
39
40 Copyright (2003) Sandia Corporation. Under the terms of Contract
41 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
42 certain rights in this software. This software is distributed under
43 the GNU General Public License.
44 ------------------------------------------------------------------------- */
45
46 /* ----------------------------------------------------------------------
47 Contributing author: Mike Brown (SNL)
48 ------------------------------------------------------------------------- */
49
50 #include <stdio.h>
51 #include <string.h>
52 #include "math_extra.h"
53
54 #define MAXJACOBI 50
55
56 namespace MathExtra {
57
58 /* ----------------------------------------------------------------------
59 output a matrix
60 ------------------------------------------------------------------------- */
61
write3(const double mat[3][3])62 void write3(const double mat[3][3])
63 {
64 for (unsigned i = 0; i < 3; i++) {
65 for (unsigned j = 0; j < 3; j++) printf("%g ",mat[i][j]);
66 printf("\n");
67 }
68 }
69
70 /* ----------------------------------------------------------------------
71 solve Ax = b or M ans = v
72 use gaussian elimination & partial pivoting on matrix
73 ------------------------------------------------------------------------- */
74
mldivide3(const double m[3][3],const double * v,double * ans)75 int mldivide3(const double m[3][3], const double *v, double *ans)
76 {
77 // create augmented matrix for pivoting
78
79 double aug[3][4];
80 for (unsigned i = 0; i < 3; i++) {
81 aug[i][3] = v[i];
82 for (unsigned j = 0; j < 3; j++) aug[i][j] = m[i][j];
83 }
84
85 for (unsigned i = 0; i < 2; i++) {
86 unsigned p = i;
87 for (unsigned j = i+1; j < 3; j++) {
88 if (fabs(aug[j][i]) > fabs(aug[i][i])) {
89 double tempv[4];
90 memcpy(tempv,aug[i],4*sizeof(double));
91 memcpy(aug[i],aug[j],4*sizeof(double));
92 memcpy(aug[j],tempv,4*sizeof(double));
93 }
94 }
95
96 while (aug[p][i] == 0.0 && p < 3) p++;
97
98 if (p == 3) return 1;
99 else
100 if (p != i) {
101 double tempv[4];
102 memcpy(tempv,aug[i],4*sizeof(double));
103 memcpy(aug[i],aug[p],4*sizeof(double));
104 memcpy(aug[p],tempv,4*sizeof(double));
105 }
106
107 for (unsigned j = i+1; j < 3; j++) {
108 double m = aug[j][i]/aug[i][i];
109 for (unsigned k=i+1; k<4; k++) aug[j][k]-=m*aug[i][k];
110 }
111 }
112
113 if (aug[2][2] == 0.0) return 1;
114
115 // back substitution
116
117 ans[2] = aug[2][3]/aug[2][2];
118 for (int i = 1; i >= 0; i--) {
119 double sumax = 0.0;
120 for (unsigned j = i+1; j < 3; j++) sumax += aug[i][j]*ans[j];
121 ans[i] = (aug[i][3]-sumax) / aug[i][i];
122 }
123
124 return 0;
125 }
126
127 /* ----------------------------------------------------------------------
128 compute evalues and evectors of 3x3 real symmetric matrix
129 based on Jacobi rotations
130 adapted from Numerical Recipes jacobi() function
131 ------------------------------------------------------------------------- */
132
jacobi(double matrix[3][3],double * evalues,double evectors[3][3])133 int jacobi(double matrix[3][3], double *evalues, double evectors[3][3])
134 {
135 int i,j,k;
136 double tresh,theta,tau,t,sm,s,h,g,c,b[3],z[3];
137
138 for (i = 0; i < 3; i++) {
139 for (j = 0; j < 3; j++) evectors[i][j] = 0.0;
140 evectors[i][i] = 1.0;
141 }
142 for (i = 0; i < 3; i++) {
143 b[i] = evalues[i] = matrix[i][i];
144 z[i] = 0.0;
145 }
146
147 for (int iter = 1; iter <= MAXJACOBI; iter++) {
148 sm = 0.0;
149 for (i = 0; i < 2; i++)
150 for (j = i+1; j < 3; j++)
151 sm += fabs(matrix[i][j]);
152 if (sm == 0.0) return 0;
153
154 if (iter < 4) tresh = 0.2*sm/(3*3);
155 else tresh = 0.0;
156
157 for (i = 0; i < 2; i++) {
158 for (j = i+1; j < 3; j++) {
159 g = 100.0*fabs(matrix[i][j]);
160 if (iter > 4 && fabs(evalues[i])+g == fabs(evalues[i])
161 && fabs(evalues[j])+g == fabs(evalues[j]))
162 matrix[i][j] = 0.0;
163 else if (fabs(matrix[i][j]) > tresh) {
164 h = evalues[j]-evalues[i];
165 if (fabs(h)+g == fabs(h)) t = (matrix[i][j])/h;
166 else {
167 theta = 0.5*h/(matrix[i][j]);
168 t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
169 if (theta < 0.0) t = -t;
170 }
171 c = 1.0/sqrt(1.0+t*t);
172 s = t*c;
173 tau = s/(1.0+c);
174 h = t*matrix[i][j];
175 z[i] -= h;
176 z[j] += h;
177 evalues[i] -= h;
178 evalues[j] += h;
179 matrix[i][j] = 0.0;
180 for (k = 0; k < i; k++) rotate(matrix,k,i,k,j,s,tau);
181 for (k = i+1; k < j; k++) rotate(matrix,i,k,k,j,s,tau);
182 for (k = j+1; k < 3; k++) rotate(matrix,i,k,j,k,s,tau);
183 for (k = 0; k < 3; k++) rotate(evectors,k,i,k,j,s,tau);
184 }
185 }
186 }
187
188 for (i = 0; i < 3; i++) {
189 evalues[i] = b[i] += z[i];
190 z[i] = 0.0;
191 }
192 }
193 return 1;
194 }
195
jacobi(double ** matrix,double * evalues,double ** evectors)196 int jacobi(double **matrix, double *evalues, double **evectors)
197 {
198 int i,j,k;
199 double tresh,theta,tau,t,sm,s,h,g,c,b[3],z[3];
200
201 for (i = 0; i < 3; i++) {
202 for (j = 0; j < 3; j++) evectors[i][j] = 0.0;
203 evectors[i][i] = 1.0;
204 }
205 for (i = 0; i < 3; i++) {
206 b[i] = evalues[i] = matrix[i][i];
207 z[i] = 0.0;
208 }
209
210 for (int iter = 1; iter <= MAXJACOBI; iter++) {
211 sm = 0.0;
212 for (i = 0; i < 2; i++)
213 for (j = i+1; j < 3; j++)
214 sm += fabs(matrix[i][j]);
215 if (sm == 0.0) return 0;
216
217 if (iter < 4) tresh = 0.2*sm/(3*3);
218 else tresh = 0.0;
219
220 for (i = 0; i < 2; i++) {
221 for (j = i+1; j < 3; j++) {
222 g = 100.0*fabs(matrix[i][j]);
223 if (iter > 4 && fabs(evalues[i])+g == fabs(evalues[i])
224 && fabs(evalues[j])+g == fabs(evalues[j]))
225 matrix[i][j] = 0.0;
226 else if (fabs(matrix[i][j]) > tresh) {
227 h = evalues[j]-evalues[i];
228 if (fabs(h)+g == fabs(h)) t = (matrix[i][j])/h;
229 else {
230 theta = 0.5*h/(matrix[i][j]);
231 t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
232 if (theta < 0.0) t = -t;
233 }
234 c = 1.0/sqrt(1.0+t*t);
235 s = t*c;
236 tau = s/(1.0+c);
237 h = t*matrix[i][j];
238 z[i] -= h;
239 z[j] += h;
240 evalues[i] -= h;
241 evalues[j] += h;
242 matrix[i][j] = 0.0;
243 for (k = 0; k < i; k++) rotate(matrix,k,i,k,j,s,tau);
244 for (k = i+1; k < j; k++) rotate(matrix,i,k,k,j,s,tau);
245 for (k = j+1; k < 3; k++) rotate(matrix,i,k,j,k,s,tau);
246 for (k = 0; k < 3; k++) rotate(evectors,k,i,k,j,s,tau);
247 }
248 }
249 }
250
251 for (i = 0; i < 3; i++) {
252 evalues[i] = b[i] += z[i];
253 z[i] = 0.0;
254 }
255 }
256 return 1;
257 }
258
259 /* ----------------------------------------------------------------------
260 perform a single Jacobi rotation
261 ------------------------------------------------------------------------- */
262
rotate(double matrix[3][3],const int i,const int j,const int k,const int l,const double s,const double tau)263 void rotate(double matrix[3][3], const int i, const int j, const int k, const int l,
264 const double s, const double tau)
265 {
266 const double g = matrix[i][j];
267 const double h = matrix[k][l];
268 matrix[i][j] = g-s*(h+g*tau);
269 matrix[k][l] = h+s*(g-h*tau);
270 }
271
rotate(double * const * const matrix,const int i,const int j,const int k,const int l,const double s,const double tau)272 void rotate(double * const * const matrix, const int i, const int j, const int k, const int l,
273 const double s, const double tau)
274 {
275 const double g = matrix[i][j];
276 const double h = matrix[k][l];
277 matrix[i][j] = g-s*(h+g*tau);
278 matrix[k][l] = h+s*(g-h*tau);
279 }
280
281 /* ----------------------------------------------------------------------
282 Richardson iteration to update quaternion from angular momentum
283 return new normalized quaternion q
284 also returns updated omega at 1/2 step
285 ------------------------------------------------------------------------- */
286
richardson(double * q,double * m,double * w,double * moments,double dtq)287 void richardson(double *q, double *m, double *w, double *moments, double dtq)
288 {
289 // full update from dq/dt = 1/2 w q
290
291 double wq[4];
292 MathExtra::vecquat(w,q,wq);
293
294 double qfull[4];
295 qfull[0] = q[0] + dtq * wq[0];
296 qfull[1] = q[1] + dtq * wq[1];
297 qfull[2] = q[2] + dtq * wq[2];
298 qfull[3] = q[3] + dtq * wq[3];
299 MathExtra::qnormalize(qfull);
300
301 // 1st half update from dq/dt = 1/2 w q
302
303 double qhalf[4];
304 qhalf[0] = q[0] + 0.5*dtq * wq[0];
305 qhalf[1] = q[1] + 0.5*dtq * wq[1];
306 qhalf[2] = q[2] + 0.5*dtq * wq[2];
307 qhalf[3] = q[3] + 0.5*dtq * wq[3];
308 MathExtra::qnormalize(qhalf);
309
310 // re-compute omega at 1/2 step from m at 1/2 step and q at 1/2 step
311 // recompute wq
312
313 MathExtra::mq_to_omega(m,qhalf,moments,w);
314 MathExtra::vecquat(w,qhalf,wq);
315
316 // 2nd half update from dq/dt = 1/2 w q
317
318 qhalf[0] += 0.5*dtq * wq[0];
319 qhalf[1] += 0.5*dtq * wq[1];
320 qhalf[2] += 0.5*dtq * wq[2];
321 qhalf[3] += 0.5*dtq * wq[3];
322 MathExtra::qnormalize(qhalf);
323
324 // corrected Richardson update
325
326 q[0] = 2.0*qhalf[0] - qfull[0];
327 q[1] = 2.0*qhalf[1] - qfull[1];
328 q[2] = 2.0*qhalf[2] - qfull[2];
329 q[3] = 2.0*qhalf[3] - qfull[3];
330 MathExtra::qnormalize(q);
331 }
332
333 /* ----------------------------------------------------------------------
334 compute omega from angular momentum, both in space frame
335 only know Idiag so need to do M = Iw in body frame
336 ex,ey,ez are column vectors of rotation matrix P
337 Mbody = P_transpose Mspace
338 wbody = Mbody / Idiag
339 wspace = P wbody
340 set wbody component to 0.0 if inertia component is 0.0
341 otherwise body can spin easily around that axis
342 ------------------------------------------------------------------------- */
343
angmom_to_omega(double * m,double * ex,double * ey,double * ez,double * idiag,double * w)344 void angmom_to_omega(double *m, double *ex, double *ey, double *ez,
345 double *idiag, double *w)
346 {
347 double wbody[3];
348
349 if (idiag[0] == 0.0) wbody[0] = 0.0;
350 else wbody[0] = (m[0]*ex[0] + m[1]*ex[1] + m[2]*ex[2]) / idiag[0];
351 if (idiag[1] == 0.0) wbody[1] = 0.0;
352 else wbody[1] = (m[0]*ey[0] + m[1]*ey[1] + m[2]*ey[2]) / idiag[1];
353 if (idiag[2] == 0.0) wbody[2] = 0.0;
354 else wbody[2] = (m[0]*ez[0] + m[1]*ez[1] + m[2]*ez[2]) / idiag[2];
355
356 w[0] = wbody[0]*ex[0] + wbody[1]*ey[0] + wbody[2]*ez[0];
357 w[1] = wbody[0]*ex[1] + wbody[1]*ey[1] + wbody[2]*ez[1];
358 w[2] = wbody[0]*ex[2] + wbody[1]*ey[2] + wbody[2]*ez[2];
359 }
360
361 /* ----------------------------------------------------------------------
362 compute omega from angular momentum
363 w = omega = angular velocity in space frame
364 wbody = angular velocity in body frame
365 project space-frame angular momentum onto body axes
366 and divide by principal moments
367 ------------------------------------------------------------------------- */
368
mq_to_omega(double * m,double * q,double * moments,double * w)369 void mq_to_omega(double *m, double *q, double *moments, double *w)
370 {
371 double wbody[3];
372 double rot[3][3];
373
374 MathExtra::quat_to_mat(q,rot);
375 MathExtra::transpose_matvec(rot,m,wbody);
376 if (moments[0] == 0.0) wbody[0] = 0.0;
377 else wbody[0] /= moments[0];
378 if (moments[1] == 0.0) wbody[1] = 0.0;
379 else wbody[1] /= moments[1];
380 if (moments[2] == 0.0) wbody[2] = 0.0;
381 else wbody[2] /= moments[2];
382 MathExtra::matvec(rot,wbody,w);
383 }
384
385 /* ----------------------------------------------------------------------
386 compute angular momentum from omega, both in space frame
387 only know Idiag so need to do M = Iw in body frame
388 ex,ey,ez are column vectors of rotation matrix P
389 wbody = P_transpose wspace
390 Mbody = Idiag wbody
391 Mspace = P Mbody
392 ------------------------------------------------------------------------- */
393
omega_to_angmom(double * w,double * ex,double * ey,double * ez,double * idiag,double * m)394 void omega_to_angmom(double *w, double *ex, double *ey, double *ez,
395 double *idiag, double *m)
396 {
397 double mbody[3];
398
399 mbody[0] = (w[0]*ex[0] + w[1]*ex[1] + w[2]*ex[2]) * idiag[0];
400 mbody[1] = (w[0]*ey[0] + w[1]*ey[1] + w[2]*ey[2]) * idiag[1];
401 mbody[2] = (w[0]*ez[0] + w[1]*ez[1] + w[2]*ez[2]) * idiag[2];
402
403 m[0] = mbody[0]*ex[0] + mbody[1]*ey[0] + mbody[2]*ez[0];
404 m[1] = mbody[0]*ex[1] + mbody[1]*ey[1] + mbody[2]*ez[1];
405 m[2] = mbody[0]*ex[2] + mbody[1]*ey[2] + mbody[2]*ez[2];
406 }
407
408 /* ----------------------------------------------------------------------
409 create unit quaternion from space-frame ex,ey,ez
410 ex,ey,ez are columns of a rotation matrix
411 ------------------------------------------------------------------------- */
412
exyz_to_q(double * ex,double * ey,double * ez,double * q)413 void exyz_to_q(double *ex, double *ey, double *ez, double *q)
414 {
415 // squares of quaternion components
416
417 double q0sq = 0.25 * (ex[0] + ey[1] + ez[2] + 1.0);
418 double q1sq = q0sq - 0.5 * (ey[1] + ez[2]);
419 double q2sq = q0sq - 0.5 * (ex[0] + ez[2]);
420 double q3sq = q0sq - 0.5 * (ex[0] + ey[1]);
421
422 // some component must be greater than 1/4 since they sum to 1
423 // compute other components from it
424
425 if (q0sq >= 0.25) {
426 q[0] = sqrt(q0sq);
427 q[1] = (ey[2] - ez[1]) / (4.0*q[0]);
428 q[2] = (ez[0] - ex[2]) / (4.0*q[0]);
429 q[3] = (ex[1] - ey[0]) / (4.0*q[0]);
430 } else if (q1sq >= 0.25) {
431 q[1] = sqrt(q1sq);
432 q[0] = (ey[2] - ez[1]) / (4.0*q[1]);
433 q[2] = (ey[0] + ex[1]) / (4.0*q[1]);
434 q[3] = (ex[2] + ez[0]) / (4.0*q[1]);
435 } else if (q2sq >= 0.25) {
436 q[2] = sqrt(q2sq);
437 q[0] = (ez[0] - ex[2]) / (4.0*q[2]);
438 q[1] = (ey[0] + ex[1]) / (4.0*q[2]);
439 q[3] = (ez[1] + ey[2]) / (4.0*q[2]);
440 } else if (q3sq >= 0.25) {
441 q[3] = sqrt(q3sq);
442 q[0] = (ex[1] - ey[0]) / (4.0*q[3]);
443 q[1] = (ez[0] + ex[2]) / (4.0*q[3]);
444 q[2] = (ez[1] + ey[2]) / (4.0*q[3]);
445 }
446
447 qnormalize(q);
448 }
449
450 /* ----------------------------------------------------------------------
451 compute space-frame ex,ey,ez from current quaternion q
452 ex,ey,ez = space-frame coords of 1st,2nd,3rd principal axis
453 operation is ex = q' d q = Q d, where d is (1,0,0) = 1st axis in body frame
454 ------------------------------------------------------------------------- */
455
q_to_exyz(double * q,double * ex,double * ey,double * ez)456 void q_to_exyz(double *q, double *ex, double *ey, double *ez)
457 {
458 ex[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
459 ex[1] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
460 ex[2] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
461
462 ey[0] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
463 ey[1] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
464 ey[2] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
465
466 ez[0] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
467 ez[1] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
468 ez[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
469 }
470
471 /* ----------------------------------------------------------------------
472 compute rotation matrix from quaternion
473 quat = [w i j k]
474 ------------------------------------------------------------------------- */
475
quat_to_mat(const double * quat,double mat[3][3])476 void quat_to_mat(const double *quat, double mat[3][3])
477 {
478 double w2 = quat[0]*quat[0];
479 double i2 = quat[1]*quat[1];
480 double j2 = quat[2]*quat[2];
481 double k2 = quat[3]*quat[3];
482 double twoij = 2.0*quat[1]*quat[2];
483 double twoik = 2.0*quat[1]*quat[3];
484 double twojk = 2.0*quat[2]*quat[3];
485 double twoiw = 2.0*quat[1]*quat[0];
486 double twojw = 2.0*quat[2]*quat[0];
487 double twokw = 2.0*quat[3]*quat[0];
488
489 mat[0][0] = w2+i2-j2-k2;
490 mat[0][1] = twoij-twokw;
491 mat[0][2] = twojw+twoik;
492
493 mat[1][0] = twoij+twokw;
494 mat[1][1] = w2-i2+j2-k2;
495 mat[1][2] = twojk-twoiw;
496
497 mat[2][0] = twoik-twojw;
498 mat[2][1] = twojk+twoiw;
499 mat[2][2] = w2-i2-j2+k2;
500 }
501
502 /* ----------------------------------------------------------------------
503 compute rotation matrix from quaternion conjugate
504 quat = [w i j k]
505 ------------------------------------------------------------------------- */
506
quat_to_mat_trans(const double * quat,double mat[3][3])507 void quat_to_mat_trans(const double *quat, double mat[3][3])
508 {
509 double w2 = quat[0]*quat[0];
510 double i2 = quat[1]*quat[1];
511 double j2 = quat[2]*quat[2];
512 double k2 = quat[3]*quat[3];
513 double twoij = 2.0*quat[1]*quat[2];
514 double twoik = 2.0*quat[1]*quat[3];
515 double twojk = 2.0*quat[2]*quat[3];
516 double twoiw = 2.0*quat[1]*quat[0];
517 double twojw = 2.0*quat[2]*quat[0];
518 double twokw = 2.0*quat[3]*quat[0];
519
520 mat[0][0] = w2+i2-j2-k2;
521 mat[1][0] = twoij-twokw;
522 mat[2][0] = twojw+twoik;
523
524 mat[0][1] = twoij+twokw;
525 mat[1][1] = w2-i2+j2-k2;
526 mat[2][1] = twojk-twoiw;
527
528 mat[0][2] = twoik-twojw;
529 mat[1][2] = twojk+twoiw;
530 mat[2][2] = w2-i2-j2+k2;
531 }
532
533 /* ----------------------------------------------------------------------
534 compute space-frame inertia tensor of an ellipsoid
535 radii = 3 radii of ellipsoid
536 quat = orientiation quaternion of ellipsoid
537 return symmetric inertia tensor as 6-vector in Voigt notation
538 ------------------------------------------------------------------------- */
539
inertia_ellipsoid(double * radii,double * quat,double mass,double * inertia)540 void inertia_ellipsoid(double *radii, double *quat, double mass,
541 double *inertia)
542 {
543 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
544 double idiag[3];
545
546 quat_to_mat(quat,p);
547 quat_to_mat_trans(quat,ptrans);
548 idiag[0] = 0.2*mass * (radii[1]*radii[1] + radii[2]*radii[2]);
549 idiag[1] = 0.2*mass * (radii[0]*radii[0] + radii[2]*radii[2]);
550 idiag[2] = 0.2*mass * (radii[0]*radii[0] + radii[1]*radii[1]);
551 diag_times3(idiag,ptrans,itemp);
552 times3(p,itemp,tensor);
553 inertia[0] = tensor[0][0];
554 inertia[1] = tensor[1][1];
555 inertia[2] = tensor[2][2];
556 inertia[3] = tensor[1][2];
557 inertia[4] = tensor[0][2];
558 inertia[5] = tensor[0][1];
559 }
560
561 /* ----------------------------------------------------------------------
562 compute space-frame inertia tensor of a line segment in 2d
563 length = length of line
564 theta = orientiation of line
565 return symmetric inertia tensor as 6-vector in Voigt notation
566 ------------------------------------------------------------------------- */
567
inertia_line(double length,double theta,double mass,double * inertia)568 void inertia_line(double length, double theta, double mass, double *inertia)
569 {
570 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
571 double q[4],idiag[3];
572
573 q[0] = cos(0.5*theta);
574 q[1] = q[2] = 0.0;
575 q[3] = sin(0.5*theta);
576 MathExtra::quat_to_mat(q,p);
577 MathExtra::quat_to_mat_trans(q,ptrans);
578 idiag[0] = 0.0;
579 idiag[1] = 1.0/12.0 * mass * length*length;
580 idiag[2] = 1.0/12.0 * mass * length*length;
581 MathExtra::diag_times3(idiag,ptrans,itemp);
582 MathExtra::times3(p,itemp,tensor);
583 inertia[0] = tensor[0][0];
584 inertia[1] = tensor[1][1];
585 inertia[2] = tensor[2][2];
586 inertia[3] = tensor[1][2];
587 inertia[4] = tensor[0][2];
588 inertia[5] = tensor[0][1];
589 }
590
591 /* ----------------------------------------------------------------------
592 compute space-frame inertia tensor of a triangle
593 v0,v1,v2 = 3 vertices of triangle
594 from http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
595 inertia tensor = a/24 (v0^2 + v1^2 + v2^2 + (v0+v1+v2)^2) I - a Vt S V
596 a = 2*area of tri = |(v1-v0) x (v2-v0)|
597 I = 3x3 identity matrix
598 V = 3x3 matrix with v0,v1,v2 as rows
599 Vt = 3x3 matrix with v0,v1,v2 as columns
600 S = 1/24 [2 1 1]
601 [1 2 1]
602 [1 1 2]
603 return symmetric inertia tensor as 6-vector in Voigt notation
604 ------------------------------------------------------------------------- */
605
inertia_triangle(double * v0,double * v1,double * v2,double mass,double * inertia)606 void inertia_triangle(double *v0, double *v1, double *v2,
607 double mass, double *inertia)
608 {
609 double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
610 double v[3][3],sv[3][3],vtsv[3][3];
611 double vvv[3],v1mv0[3],v2mv0[3],normal[3];
612
613 v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
614 v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
615 v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
616
617 times3(s,v,sv);
618 transpose_times3(v,sv,vtsv);
619
620 double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
621 vvv[0] = v0[0] + v1[0] + v2[0];
622 vvv[1] = v0[1] + v1[1] + v2[1];
623 vvv[2] = v0[2] + v1[2] + v2[2];
624 sum += lensq3(vvv);
625
626 sub3(v1,v0,v1mv0);
627 sub3(v2,v0,v2mv0);
628 cross3(v1mv0,v2mv0,normal);
629 double a = len3(normal);
630 double inv24 = mass/24.0;
631
632 inertia[0] = inv24*a*(sum-vtsv[0][0]);
633 inertia[1] = inv24*a*(sum-vtsv[1][1]);
634 inertia[2] = inv24*a*(sum-vtsv[2][2]);
635 inertia[3] = -inv24*a*vtsv[1][2];
636 inertia[4] = -inv24*a*vtsv[0][2];
637 inertia[5] = -inv24*a*vtsv[0][1];
638 }
639
640 /* ----------------------------------------------------------------------
641 compute space-frame inertia tensor of a triangle
642 idiag = previously computed diagonal inertia tensor
643 quat = orientiation quaternion of triangle
644 return symmetric inertia tensor as 6-vector in Voigt notation
645 ------------------------------------------------------------------------- */
646
inertia_triangle(double * idiag,double * quat,double mass,double * inertia)647 void inertia_triangle(double *idiag, double *quat, double mass,
648 double *inertia)
649 {
650 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
651
652 quat_to_mat(quat,p);
653 quat_to_mat_trans(quat,ptrans);
654 diag_times3(idiag,ptrans,itemp);
655 times3(p,itemp,tensor);
656 inertia[0] = tensor[0][0];
657 inertia[1] = tensor[1][1];
658 inertia[2] = tensor[2][2];
659 inertia[3] = tensor[1][2];
660 inertia[4] = tensor[0][2];
661 inertia[5] = tensor[0][1];
662 }
663
664 /* ---------------------------------------------------------------------- */
665
666 }
667