1 // clang-format off
2 /* ----------------------------------------------------------------------
3 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
4 https://www.lammps.org/, Sandia National Laboratories
5 Steve Plimpton, sjplimp@sandia.gov
6
7 Copyright (2003) Sandia Corporation. Under the terms of Contract
8 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
9 certain rights in this software. This software is distributed under
10 the GNU General Public License.
11
12 See the README file in the top-level LAMMPS directory.
13 ------------------------------------------------------------------------- */
14
15 /* ----------------------------------------------------------------------
16 Contributing author: Mike Brown (SNL)
17 ------------------------------------------------------------------------- */
18
19 #include "math_extra.h"
20 #include <cstdio>
21 #include <cstring>
22
23 namespace MathExtra {
24
25 /* ----------------------------------------------------------------------
26 output a matrix
27 ------------------------------------------------------------------------- */
28
write3(const double mat[3][3])29 void write3(const double mat[3][3])
30 {
31 for (unsigned i = 0; i < 3; i++) {
32 for (unsigned j = 0; j < 3; j++) printf("%g ",mat[i][j]);
33 printf("\n");
34 }
35 }
36
37 /* ----------------------------------------------------------------------
38 solve Ax = b or M ans = v
39 use gaussian elimination & partial pivoting on matrix
40 ------------------------------------------------------------------------- */
41
mldivide3(const double m[3][3],const double * v,double * ans)42 int mldivide3(const double m[3][3], const double *v, double *ans)
43 {
44 // create augmented matrix for pivoting
45
46 double aug[3][4];
47 for (unsigned i = 0; i < 3; i++) {
48 aug[i][3] = v[i];
49 for (unsigned j = 0; j < 3; j++) aug[i][j] = m[i][j];
50 }
51
52 for (unsigned i = 0; i < 2; i++) {
53 unsigned p = i;
54 for (unsigned j = i+1; j < 3; j++) {
55 if (fabs(aug[j][i]) > fabs(aug[i][i])) {
56 double tempv[4];
57 memcpy(tempv,aug[i],4*sizeof(double));
58 memmove(aug[i],aug[j],4*sizeof(double));
59 memcpy(aug[j],tempv,4*sizeof(double));
60 }
61 }
62
63 while (p < 3 && aug[p][i] == 0.0) p++;
64
65 if (p == 3) return 1;
66 else
67 if (p != i) {
68 double tempv[4];
69 memcpy(tempv,aug[i],4*sizeof(double));
70 memmove(aug[i],aug[p],4*sizeof(double));
71 memcpy(aug[p],tempv,4*sizeof(double));
72 }
73
74 for (unsigned j = i+1; j < 3; j++) {
75 double n = aug[j][i]/aug[i][i];
76 for (unsigned k=i+1; k<4; k++) aug[j][k]-=n*aug[i][k];
77 }
78 }
79
80 if (aug[2][2] == 0.0) return 1;
81
82 // back substitution
83
84 ans[2] = aug[2][3]/aug[2][2];
85 for (int i = 1; i >= 0; i--) {
86 double sumax = 0.0;
87 for (unsigned j = i+1; j < 3; j++) sumax += aug[i][j]*ans[j];
88 ans[i] = (aug[i][3]-sumax) / aug[i][i];
89 }
90
91 return 0;
92 }
93
94 /* ----------------------------------------------------------------------
95 Richardson iteration to update quaternion from angular momentum
96 return new normalized quaternion q
97 also returns updated omega at 1/2 step
98 ------------------------------------------------------------------------- */
99
richardson(double * q,double * m,double * w,double * moments,double dtq)100 void richardson(double *q, double *m, double *w, double *moments, double dtq)
101 {
102 // full update from dq/dt = 1/2 w q
103
104 double wq[4];
105 MathExtra::vecquat(w,q,wq);
106
107 double qfull[4];
108 qfull[0] = q[0] + dtq * wq[0];
109 qfull[1] = q[1] + dtq * wq[1];
110 qfull[2] = q[2] + dtq * wq[2];
111 qfull[3] = q[3] + dtq * wq[3];
112 MathExtra::qnormalize(qfull);
113
114 // 1st half update from dq/dt = 1/2 w q
115
116 double qhalf[4];
117 qhalf[0] = q[0] + 0.5*dtq * wq[0];
118 qhalf[1] = q[1] + 0.5*dtq * wq[1];
119 qhalf[2] = q[2] + 0.5*dtq * wq[2];
120 qhalf[3] = q[3] + 0.5*dtq * wq[3];
121 MathExtra::qnormalize(qhalf);
122
123 // re-compute omega at 1/2 step from m at 1/2 step and q at 1/2 step
124 // recompute wq
125
126 MathExtra::mq_to_omega(m,qhalf,moments,w);
127 MathExtra::vecquat(w,qhalf,wq);
128
129 // 2nd half update from dq/dt = 1/2 w q
130
131 qhalf[0] += 0.5*dtq * wq[0];
132 qhalf[1] += 0.5*dtq * wq[1];
133 qhalf[2] += 0.5*dtq * wq[2];
134 qhalf[3] += 0.5*dtq * wq[3];
135 MathExtra::qnormalize(qhalf);
136
137 // corrected Richardson update
138
139 q[0] = 2.0*qhalf[0] - qfull[0];
140 q[1] = 2.0*qhalf[1] - qfull[1];
141 q[2] = 2.0*qhalf[2] - qfull[2];
142 q[3] = 2.0*qhalf[3] - qfull[3];
143 MathExtra::qnormalize(q);
144 }
145
146 /* ----------------------------------------------------------------------
147 apply evolution operators to quat, quat momentum
148 Miller et al., J Chem Phys. 116, 8649-8659 (2002)
149 ------------------------------------------------------------------------- */
150
no_squish_rotate(int k,double * p,double * q,double * inertia,double dt)151 void no_squish_rotate(int k, double *p, double *q, double *inertia,
152 double dt)
153 {
154 double phi,c_phi,s_phi,kp[4],kq[4];
155
156 // apply permuation operator on p and q, get kp and kq
157
158 if (k == 1) {
159 kq[0] = -q[1]; kp[0] = -p[1];
160 kq[1] = q[0]; kp[1] = p[0];
161 kq[2] = q[3]; kp[2] = p[3];
162 kq[3] = -q[2]; kp[3] = -p[2];
163 } else if (k == 2) {
164 kq[0] = -q[2]; kp[0] = -p[2];
165 kq[1] = -q[3]; kp[1] = -p[3];
166 kq[2] = q[0]; kp[2] = p[0];
167 kq[3] = q[1]; kp[3] = p[1];
168 } else if (k == 3) {
169 kq[0] = -q[3]; kp[0] = -p[3];
170 kq[1] = q[2]; kp[1] = p[2];
171 kq[2] = -q[1]; kp[2] = -p[1];
172 kq[3] = q[0]; kp[3] = p[0];
173 }
174
175 // obtain phi, cosines and sines
176
177 phi = p[0]*kq[0] + p[1]*kq[1] + p[2]*kq[2] + p[3]*kq[3];
178 if (inertia[k-1] == 0.0) phi = 0.0;
179 else phi /= 4.0 * inertia[k-1];
180 c_phi = cos(dt * phi);
181 s_phi = sin(dt * phi);
182
183 // advance p and q
184
185 p[0] = c_phi*p[0] + s_phi*kp[0];
186 p[1] = c_phi*p[1] + s_phi*kp[1];
187 p[2] = c_phi*p[2] + s_phi*kp[2];
188 p[3] = c_phi*p[3] + s_phi*kp[3];
189
190 q[0] = c_phi*q[0] + s_phi*kq[0];
191 q[1] = c_phi*q[1] + s_phi*kq[1];
192 q[2] = c_phi*q[2] + s_phi*kq[2];
193 q[3] = c_phi*q[3] + s_phi*kq[3];
194 }
195
196 /* ----------------------------------------------------------------------
197 compute omega from angular momentum, both in space frame
198 only know Idiag so need to do M = Iw in body frame
199 ex,ey,ez are column vectors of rotation matrix P
200 Mbody = P_transpose Mspace
201 wbody = Mbody / Idiag
202 wspace = P wbody
203 set wbody component to 0.0 if inertia component is 0.0
204 otherwise body can spin easily around that axis
205 ------------------------------------------------------------------------- */
206
angmom_to_omega(double * m,double * ex,double * ey,double * ez,double * idiag,double * w)207 void angmom_to_omega(double *m, double *ex, double *ey, double *ez,
208 double *idiag, double *w)
209 {
210 double wbody[3];
211
212 if (idiag[0] == 0.0) wbody[0] = 0.0;
213 else wbody[0] = (m[0]*ex[0] + m[1]*ex[1] + m[2]*ex[2]) / idiag[0];
214 if (idiag[1] == 0.0) wbody[1] = 0.0;
215 else wbody[1] = (m[0]*ey[0] + m[1]*ey[1] + m[2]*ey[2]) / idiag[1];
216 if (idiag[2] == 0.0) wbody[2] = 0.0;
217 else wbody[2] = (m[0]*ez[0] + m[1]*ez[1] + m[2]*ez[2]) / idiag[2];
218
219 w[0] = wbody[0]*ex[0] + wbody[1]*ey[0] + wbody[2]*ez[0];
220 w[1] = wbody[0]*ex[1] + wbody[1]*ey[1] + wbody[2]*ez[1];
221 w[2] = wbody[0]*ex[2] + wbody[1]*ey[2] + wbody[2]*ez[2];
222 }
223
224 /* ----------------------------------------------------------------------
225 compute omega from angular momentum
226 w = omega = angular velocity in space frame
227 wbody = angular velocity in body frame
228 project space-frame angular momentum onto body axes
229 and divide by principal moments
230 ------------------------------------------------------------------------- */
231
mq_to_omega(double * m,double * q,double * moments,double * w)232 void mq_to_omega(double *m, double *q, double *moments, double *w)
233 {
234 double wbody[3];
235 double rot[3][3];
236
237 MathExtra::quat_to_mat(q,rot);
238 MathExtra::transpose_matvec(rot,m,wbody);
239 if (moments[0] == 0.0) wbody[0] = 0.0;
240 else wbody[0] /= moments[0];
241 if (moments[1] == 0.0) wbody[1] = 0.0;
242 else wbody[1] /= moments[1];
243 if (moments[2] == 0.0) wbody[2] = 0.0;
244 else wbody[2] /= moments[2];
245 MathExtra::matvec(rot,wbody,w);
246 }
247
248 /* ----------------------------------------------------------------------
249 compute angular momentum from omega, both in space frame
250 only know Idiag so need to do M = Iw in body frame
251 ex,ey,ez are column vectors of rotation matrix P
252 wbody = P_transpose wspace
253 Mbody = Idiag wbody
254 Mspace = P Mbody
255 ------------------------------------------------------------------------- */
256
omega_to_angmom(double * w,double * ex,double * ey,double * ez,double * idiag,double * m)257 void omega_to_angmom(double *w, double *ex, double *ey, double *ez,
258 double *idiag, double *m)
259 {
260 double mbody[3];
261
262 mbody[0] = (w[0]*ex[0] + w[1]*ex[1] + w[2]*ex[2]) * idiag[0];
263 mbody[1] = (w[0]*ey[0] + w[1]*ey[1] + w[2]*ey[2]) * idiag[1];
264 mbody[2] = (w[0]*ez[0] + w[1]*ez[1] + w[2]*ez[2]) * idiag[2];
265
266 m[0] = mbody[0]*ex[0] + mbody[1]*ey[0] + mbody[2]*ez[0];
267 m[1] = mbody[0]*ex[1] + mbody[1]*ey[1] + mbody[2]*ez[1];
268 m[2] = mbody[0]*ex[2] + mbody[1]*ey[2] + mbody[2]*ez[2];
269 }
270
271 /* ----------------------------------------------------------------------
272 create unit quaternion from space-frame ex,ey,ez
273 ex,ey,ez are columns of a rotation matrix
274 ------------------------------------------------------------------------- */
275
exyz_to_q(double * ex,double * ey,double * ez,double * q)276 void exyz_to_q(double *ex, double *ey, double *ez, double *q)
277 {
278 // squares of quaternion components
279
280 double q0sq = 0.25 * (ex[0] + ey[1] + ez[2] + 1.0);
281 double q1sq = q0sq - 0.5 * (ey[1] + ez[2]);
282 double q2sq = q0sq - 0.5 * (ex[0] + ez[2]);
283 double q3sq = q0sq - 0.5 * (ex[0] + ey[1]);
284
285 // some component must be greater than 1/4 since they sum to 1
286 // compute other components from it
287
288 if (q0sq >= 0.25) {
289 q[0] = sqrt(q0sq);
290 q[1] = (ey[2] - ez[1]) / (4.0*q[0]);
291 q[2] = (ez[0] - ex[2]) / (4.0*q[0]);
292 q[3] = (ex[1] - ey[0]) / (4.0*q[0]);
293 } else if (q1sq >= 0.25) {
294 q[1] = sqrt(q1sq);
295 q[0] = (ey[2] - ez[1]) / (4.0*q[1]);
296 q[2] = (ey[0] + ex[1]) / (4.0*q[1]);
297 q[3] = (ex[2] + ez[0]) / (4.0*q[1]);
298 } else if (q2sq >= 0.25) {
299 q[2] = sqrt(q2sq);
300 q[0] = (ez[0] - ex[2]) / (4.0*q[2]);
301 q[1] = (ey[0] + ex[1]) / (4.0*q[2]);
302 q[3] = (ez[1] + ey[2]) / (4.0*q[2]);
303 } else if (q3sq >= 0.25) {
304 q[3] = sqrt(q3sq);
305 q[0] = (ex[1] - ey[0]) / (4.0*q[3]);
306 q[1] = (ez[0] + ex[2]) / (4.0*q[3]);
307 q[2] = (ez[1] + ey[2]) / (4.0*q[3]);
308 }
309
310 qnormalize(q);
311 }
312
313 /* ----------------------------------------------------------------------
314 compute space-frame ex,ey,ez from current quaternion q
315 ex,ey,ez = space-frame coords of 1st,2nd,3rd principal axis
316 operation is ex = q' d q = Q d, where d is (1,0,0) = 1st axis in body frame
317 ------------------------------------------------------------------------- */
318
q_to_exyz(double * q,double * ex,double * ey,double * ez)319 void q_to_exyz(double *q, double *ex, double *ey, double *ez)
320 {
321 ex[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
322 ex[1] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
323 ex[2] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
324
325 ey[0] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
326 ey[1] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
327 ey[2] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
328
329 ez[0] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
330 ez[1] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
331 ez[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
332 }
333
334 /* ----------------------------------------------------------------------
335 compute rotation matrix from quaternion
336 quat = [w i j k]
337 ------------------------------------------------------------------------- */
338
quat_to_mat(const double * quat,double mat[3][3])339 void quat_to_mat(const double *quat, double mat[3][3])
340 {
341 double w2 = quat[0]*quat[0];
342 double i2 = quat[1]*quat[1];
343 double j2 = quat[2]*quat[2];
344 double k2 = quat[3]*quat[3];
345 double twoij = 2.0*quat[1]*quat[2];
346 double twoik = 2.0*quat[1]*quat[3];
347 double twojk = 2.0*quat[2]*quat[3];
348 double twoiw = 2.0*quat[1]*quat[0];
349 double twojw = 2.0*quat[2]*quat[0];
350 double twokw = 2.0*quat[3]*quat[0];
351
352 mat[0][0] = w2+i2-j2-k2;
353 mat[0][1] = twoij-twokw;
354 mat[0][2] = twojw+twoik;
355
356 mat[1][0] = twoij+twokw;
357 mat[1][1] = w2-i2+j2-k2;
358 mat[1][2] = twojk-twoiw;
359
360 mat[2][0] = twoik-twojw;
361 mat[2][1] = twojk+twoiw;
362 mat[2][2] = w2-i2-j2+k2;
363 }
364
365 /* ----------------------------------------------------------------------
366 compute rotation matrix from quaternion conjugate
367 quat = [w i j k]
368 ------------------------------------------------------------------------- */
369
quat_to_mat_trans(const double * quat,double mat[3][3])370 void quat_to_mat_trans(const double *quat, double mat[3][3])
371 {
372 double w2 = quat[0]*quat[0];
373 double i2 = quat[1]*quat[1];
374 double j2 = quat[2]*quat[2];
375 double k2 = quat[3]*quat[3];
376 double twoij = 2.0*quat[1]*quat[2];
377 double twoik = 2.0*quat[1]*quat[3];
378 double twojk = 2.0*quat[2]*quat[3];
379 double twoiw = 2.0*quat[1]*quat[0];
380 double twojw = 2.0*quat[2]*quat[0];
381 double twokw = 2.0*quat[3]*quat[0];
382
383 mat[0][0] = w2+i2-j2-k2;
384 mat[1][0] = twoij-twokw;
385 mat[2][0] = twojw+twoik;
386
387 mat[0][1] = twoij+twokw;
388 mat[1][1] = w2-i2+j2-k2;
389 mat[2][1] = twojk-twoiw;
390
391 mat[0][2] = twoik-twojw;
392 mat[1][2] = twojk+twoiw;
393 mat[2][2] = w2-i2-j2+k2;
394 }
395
396 /* ----------------------------------------------------------------------
397 compute space-frame inertia tensor of an ellipsoid
398 radii = 3 radii of ellipsoid
399 quat = orientiation quaternion of ellipsoid
400 return symmetric inertia tensor as 6-vector in Voigt ordering
401 ------------------------------------------------------------------------- */
402
inertia_ellipsoid(double * radii,double * quat,double mass,double * inertia)403 void inertia_ellipsoid(double *radii, double *quat, double mass,
404 double *inertia)
405 {
406 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
407 double idiag[3];
408
409 quat_to_mat(quat,p);
410 quat_to_mat_trans(quat,ptrans);
411 idiag[0] = 0.2*mass * (radii[1]*radii[1] + radii[2]*radii[2]);
412 idiag[1] = 0.2*mass * (radii[0]*radii[0] + radii[2]*radii[2]);
413 idiag[2] = 0.2*mass * (radii[0]*radii[0] + radii[1]*radii[1]);
414 diag_times3(idiag,ptrans,itemp);
415 times3(p,itemp,tensor);
416 inertia[0] = tensor[0][0];
417 inertia[1] = tensor[1][1];
418 inertia[2] = tensor[2][2];
419 inertia[3] = tensor[1][2];
420 inertia[4] = tensor[0][2];
421 inertia[5] = tensor[0][1];
422 }
423
424 /* ----------------------------------------------------------------------
425 compute space-frame inertia tensor of a line segment in 2d
426 length = length of line
427 theta = orientiation of line
428 return symmetric inertia tensor as 6-vector in Voigt ordering
429 ------------------------------------------------------------------------- */
430
inertia_line(double length,double theta,double mass,double * inertia)431 void inertia_line(double length, double theta, double mass, double *inertia)
432 {
433 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
434 double q[4],idiag[3];
435
436 q[0] = cos(0.5*theta);
437 q[1] = q[2] = 0.0;
438 q[3] = sin(0.5*theta);
439 MathExtra::quat_to_mat(q,p);
440 MathExtra::quat_to_mat_trans(q,ptrans);
441 idiag[0] = 0.0;
442 idiag[1] = 1.0/12.0 * mass * length*length;
443 idiag[2] = 1.0/12.0 * mass * length*length;
444 MathExtra::diag_times3(idiag,ptrans,itemp);
445 MathExtra::times3(p,itemp,tensor);
446 inertia[0] = tensor[0][0];
447 inertia[1] = tensor[1][1];
448 inertia[2] = tensor[2][2];
449 inertia[3] = tensor[1][2];
450 inertia[4] = tensor[0][2];
451 inertia[5] = tensor[0][1];
452 }
453
454 /* ----------------------------------------------------------------------
455 compute space-frame inertia tensor of a triangle
456 v0,v1,v2 = 3 vertices of triangle
457 from https://en.wikipedia.org/wiki/List_of_moments_of_inertia
458 inertia tensor = a/24 (v0^2 + v1^2 + v2^2 + (v0+v1+v2)^2) I - a Vt S V
459 a = 2*area of tri = |(v1-v0) x (v2-v0)|
460 I = 3x3 identity matrix
461 V = 3x3 matrix with v0,v1,v2 as rows
462 Vt = 3x3 matrix with v0,v1,v2 as columns
463 S = 1/24 [2 1 1]
464 [1 2 1]
465 [1 1 2]
466 return symmetric inertia tensor as 6-vector in Voigt ordering
467 ------------------------------------------------------------------------- */
468
inertia_triangle(double * v0,double * v1,double * v2,double mass,double * inertia)469 void inertia_triangle(double *v0, double *v1, double *v2,
470 double mass, double *inertia)
471 {
472 double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
473 double v[3][3],sv[3][3],vtsv[3][3];
474 double vvv[3],v1mv0[3],v2mv0[3],normal[3];
475
476 v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
477 v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
478 v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
479
480 times3(s,v,sv);
481 transpose_times3(v,sv,vtsv);
482
483 double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
484 vvv[0] = v0[0] + v1[0] + v2[0];
485 vvv[1] = v0[1] + v1[1] + v2[1];
486 vvv[2] = v0[2] + v1[2] + v2[2];
487 sum += lensq3(vvv);
488
489 sub3(v1,v0,v1mv0);
490 sub3(v2,v0,v2mv0);
491 cross3(v1mv0,v2mv0,normal);
492 double a = len3(normal);
493 double inv24 = mass/24.0;
494
495 inertia[0] = inv24*a*(sum-vtsv[0][0]);
496 inertia[1] = inv24*a*(sum-vtsv[1][1]);
497 inertia[2] = inv24*a*(sum-vtsv[2][2]);
498 inertia[3] = -inv24*a*vtsv[1][2];
499 inertia[4] = -inv24*a*vtsv[0][2];
500 inertia[5] = -inv24*a*vtsv[0][1];
501 }
502
503 /* ----------------------------------------------------------------------
504 compute space-frame inertia tensor of a triangle
505 idiag = previously computed diagonal inertia tensor
506 quat = orientiation quaternion of triangle
507 return symmetric inertia tensor as 6-vector in Voigt ordering
508 ------------------------------------------------------------------------- */
509
inertia_triangle(double * idiag,double * quat,double,double * inertia)510 void inertia_triangle(double *idiag, double *quat, double /*mass*/,
511 double *inertia)
512 {
513 double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
514
515 quat_to_mat(quat,p);
516 quat_to_mat_trans(quat,ptrans);
517 diag_times3(idiag,ptrans,itemp);
518 times3(p,itemp,tensor);
519 inertia[0] = tensor[0][0];
520 inertia[1] = tensor[1][1];
521 inertia[2] = tensor[2][2];
522 inertia[3] = tensor[1][2];
523 inertia[4] = tensor[0][2];
524 inertia[5] = tensor[0][1];
525 }
526
527 /* ----------------------------------------------------------------------
528 Build rotation matrix for a small angle rotation around the X axis
529 ------------------------------------------------------------------------- */
530
BuildRxMatrix(double R[3][3],const double angle)531 void BuildRxMatrix(double R[3][3], const double angle)
532 {
533 const double angleSq = angle * angle;
534 const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
535 const double sinAngle = angle / (1.0 + angleSq * 0.25);
536
537 R[0][0] = 1.0; R[0][1] = 0.0; R[0][2] = 0.0;
538 R[1][0] = 0.0; R[1][1] = cosAngle; R[1][2] = -sinAngle;
539 R[2][0] = 0.0; R[2][1] = sinAngle; R[2][2] = cosAngle;
540 }
541
542 /* ----------------------------------------------------------------------
543 Build rotation matrix for a small angle rotation around the Y axis
544 ------------------------------------------------------------------------- */
545
BuildRyMatrix(double R[3][3],const double angle)546 void BuildRyMatrix(double R[3][3], const double angle)
547 {
548 const double angleSq = angle * angle;
549 const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
550 const double sinAngle = angle / (1.0 + angleSq * 0.25);
551
552 R[0][0] = cosAngle; R[0][1] = 0.0; R[0][2] = sinAngle;
553 R[1][0] = 0.0; R[1][1] = 1.0; R[1][2] = 0.0;
554 R[2][0] = -sinAngle; R[2][1] = 0.0; R[2][2] = cosAngle;
555 }
556
557 /* ----------------------------------------------------------------------
558 Build rotation matrix for a small angle rotation around the Z axis
559 ------------------------------------------------------------------------- */
560
BuildRzMatrix(double R[3][3],const double angle)561 void BuildRzMatrix(double R[3][3], const double angle)
562 {
563 const double angleSq = angle * angle;
564 const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
565 const double sinAngle = angle / (1.0 + angleSq * 0.25);
566
567 R[0][0] = cosAngle; R[0][1] = -sinAngle; R[0][2] = 0.0;
568 R[1][0] = sinAngle; R[1][1] = cosAngle; R[1][2] = 0.0;
569 R[2][0] = 0.0; R[2][1] = 0.0; R[2][2] = 1.0;
570 }
571
572 /* ---------------------------------------------------------------------- */
573
574 }
575