b2api
B2000++ API Reference Manual, VERSION 4.6
 
Loading...
Searching...
No Matches
b2finite_rotation.H
1//------------------------------------------------------------------------
2// b2finite_rotation.H --
3//
4// written by Mathias Doreille
5//
6// Copyright (c) 2011-2012,2017
7// SMR Engineering & Development SA
8// 2502 Bienne, Switzerland
9//
10// All Rights Reserved. Proprietary source code. The contents of
11// this file may not be disclosed to third parties, copied or
12// duplicated in any form, in whole or in part, without the prior
13// written permission of SMR.
14//------------------------------------------------------------------------
15
16#ifndef _B2FINITE_ROTATION_H_
17#define _B2FINITE_ROTATION_H_
18
19#include <algorithm>
20#include <cassert>
21#include <cmath>
22
23#include "b2constants.H"
24#include "b2tensor_calculus.H"
25
26namespace b2000 {
27
28// The finite rotation and derivatives come from the article:
29//
30// On the differentiation of the Rodrigues formula and its significance for
31// the vector-like parameterization of Reissner-Simo beam theory,
32// M. Ritto-Corrêa, and D. Camotim,
33// International Journal for Numerical Methods in Engineering,
34// Volume 55, Issue 9, pages 1005-1032, 30 November 2002
35//
36template <typename TP>
37class FiniteRotation {
38public:
39 FiniteRotation()
40 : omega(0),
41 nomega(0),
42 adef_min(0),
43 adef_max(0),
44 bdef_min(0),
45 bdef_max(0),
46 cdef_min(0),
47 cdef_max(0) {}
48
49 FiniteRotation(const TP omega_[3])
50 : omega(omega_),
51 // set nomega to zero if it is not a number to avoid
52 // infinite loop in the get_x
53 nomega(zero_if_nan(std::sqrt(dot_3(omega_, omega_)))),
54 adef_min(0),
55 adef_max(0),
56 bdef_min(0),
57 bdef_max(0),
58 cdef_min(0),
59 cdef_max(0) {}
60
61 void init(const TP omega_[3]) {
62 omega = omega_;
63 nomega = zero_if_nan(std::sqrt(dot_3(omega_, omega_)));
64 }
65
66 void get_rotator(TP rotator[3][3]) {
67 compute_a(0, 3);
68 for (int j = 0; j != 3; ++j) {
69 for (int i = 0; i != 3; ++i) { rotator[j][i] = a[2] * omega[i] * omega[j]; }
70 rotator[j][j] += a[0];
71 }
72 const TP w[] = {omega[0] * a[1], omega[1] * a[1], omega[2] * a[1]};
73 add_skew_matrix(w, rotator);
74 }
75
76 void get_d_rotator_d_omega_in_u(const TP u[3], TP d_rotator_in_u[3][3]) {
77 compute_a(1, 3);
78 compute_b(0, 3);
79 const TP omega_u = dot_3(omega, u);
80 const TP s_ident = b[0] * omega_u;
81 const TP s_cross_omega = b[2] * omega_u;
82 for (int j = 0; j != 3; ++j) {
83 for (int i = 0; i != 3; ++i) {
84 d_rotator_in_u[j][i] = a[2] * (u[i] * omega[j] + u[j] * omega[i])
85 + s_cross_omega * omega[i] * omega[j];
86 }
87 d_rotator_in_u[j][j] += s_ident;
88 }
89 {
90 TP w[3];
91 const TP omega_s = b[1] * omega_u;
92 for (int i = 0; i != 3; ++i) { w[i] = a[1] * u[i] + omega_s * omega[i]; }
93 add_skew_matrix(w, d_rotator_in_u);
94 }
95 }
96
97 void get_d_trans_rotator_d_omega_in_u(const TP u[3], TP d_trans_rotator_in_u[3][3]) {
98 compute_a(1, 3);
99 compute_b(0, 3);
100 const TP omega_u = dot_3(omega, u);
101 const TP s_ident = b[0] * omega_u;
102 const TP s_cross_omega = b[2] * omega_u;
103 for (int j = 0; j != 3; ++j) {
104 for (int i = 0; i != 3; ++i) {
105 d_trans_rotator_in_u[j][i] = a[2] * (u[i] * omega[j] + u[j] * omega[i])
106 + s_cross_omega * omega[i] * omega[j];
107 }
108 d_trans_rotator_in_u[j][j] += s_ident;
109 }
110 {
111 TP w[3];
112 const TP omega_s = -b[1] * omega_u;
113 for (int i = 0; i != 3; ++i) { w[i] = -a[1] * u[i] + omega_s * omega[i]; }
114 add_skew_matrix(w, d_trans_rotator_in_u);
115 }
116 }
117
118 void get_d2_rotator_d2_omega_in_uv(const TP u[3], const TP v[3], TP d_rotator_in_uv[3][3]) {
119 compute_a(2, 3);
120 compute_b(0, 3);
121 compute_c(0, 3);
122 const TP u_v = dot_3(u, v);
123 const TP omega_u = dot_3(omega, u);
124 const TP omega_v = dot_3(omega, v);
125 const TP omega_v_omega_u = omega_v * omega_u;
126 const TP b2_omega_u = b[2] * omega_u;
127 const TP b2_omega_v = b[2] * omega_v;
128 const TP b2_u_v_c2_omega_v_omega_u = b[2] * u_v + c[2] * omega_v_omega_u;
129 const TP s_ident = b[0] * u_v + c[0] * omega_v_omega_u;
130 for (int j = 0; j != 3; ++j) {
131 for (int i = 0; i != 3; ++i) {
132 d_rotator_in_uv[j][i] = a[2] * (u[i] * v[j] + u[j] * v[i])
133 + b2_omega_u * (v[i] * omega[j] + v[j] * omega[i])
134 + b2_omega_v * (u[i] * omega[j] + u[j] * omega[i])
135 + b2_u_v_c2_omega_v_omega_u * omega[i] * omega[j];
136 }
137 d_rotator_in_uv[j][j] += s_ident;
138 }
139 {
140 TP w[3];
141 const TP us = b[1] * omega_v;
142 const TP vs = b[1] * omega_u;
143 const TP omega_s = b[1] * u_v + c[1] * omega_v_omega_u;
144 for (int i = 0; i != 3; ++i) { w[i] = us * u[i] + vs * v[i] + omega_s * omega[i]; }
145 add_skew_matrix(w, d_rotator_in_uv);
146 }
147 }
148
149 // return T, the variation of the spin vector in function of
150 // the variation of omega at omega
151 void get_T(TP T[3][3]) {
152 compute_a(1, 4);
153 for (int j = 0; j != 3; ++j) {
154 for (int i = 0; i != 3; ++i) { T[j][i] = a[3] * omega[i] * omega[j]; }
155 T[j][j] += a[1];
156 }
157 const TP w[] = {omega[0] * a[2], omega[1] * a[2], omega[2] * a[2]};
158 add_skew_matrix(w, T);
159 }
160
161 void get_d_T_d_omega_in_u(const TP u[3], TP d_T_in_u[3][3]) {
162 compute_a(2, 4);
163 compute_b(1, 4);
164 const TP omega_u = dot_3(omega, u);
165 const TP s_ident = b[1] * omega_u;
166 const TP s_cross_omega = b[3] * omega_u;
167 for (int j = 0; j != 3; ++j) {
168 for (int i = 0; i != 3; ++i) {
169 d_T_in_u[j][i] = a[3] * (u[i] * omega[j] + u[j] * omega[i])
170 + s_cross_omega * omega[i] * omega[j];
171 }
172 d_T_in_u[j][j] += s_ident;
173 }
174 {
175 TP w[3];
176 const TP omega_s = b[2] * omega_u;
177 for (int i = 0; i != 3; ++i) { w[i] = a[2] * u[i] + omega_s * omega[i]; }
178 add_skew_matrix(w, d_T_in_u);
179 }
180 }
181
182 void get_d_trans_T_d_omega_in_u(const TP u[3], TP d_trans_T_in_u[3][3]) {
183 compute_a(2, 4);
184 compute_b(1, 4);
185 const TP omega_u = dot_3(omega, u);
186 const TP s_ident = b[1] * omega_u;
187 const TP s_cross_omega = b[3] * omega_u;
188 for (int j = 0; j != 3; ++j) {
189 for (int i = 0; i != 3; ++i) {
190 d_trans_T_in_u[j][i] = a[3] * (u[i] * omega[j] + u[j] * omega[i])
191 + s_cross_omega * omega[i] * omega[j];
192 }
193 d_trans_T_in_u[j][j] += s_ident;
194 }
195 {
196 TP w[3];
197 const TP omega_s = -b[2] * omega_u;
198 for (int i = 0; i != 3; ++i) { w[i] = -a[2] * u[i] + omega_s * omega[i]; }
199 add_skew_matrix(w, d_trans_T_in_u);
200 }
201 }
202
203 void get_d2_T_d2_omega_in_uv(const TP u[3], const TP v[3], TP d_T_in_uv[3][3]) {
204 compute_a(3, 4);
205 compute_b(1, 4);
206 compute_c(1, 4);
207 const TP u_v = dot_3(u, v);
208 const TP omega_u = dot_3(omega, u);
209 const TP omega_v = dot_3(omega, v);
210 const TP omega_v_omega_u = omega_v * omega_u;
211 const TP b2_omega_u = b[3] * omega_u;
212 const TP b2_omega_v = b[3] * omega_v;
213 const TP b2_u_v_c2_omega_v_omega_u = b[3] * u_v + c[3] * omega_v_omega_u;
214 const TP s_ident = b[1] * u_v + c[1] * omega_v_omega_u;
215 for (int j = 0; j != 3; ++j) {
216 for (int i = 0; i != 3; ++i) {
217 d_T_in_uv[j][i] = a[3] * (u[i] * v[j] + u[j] * v[i])
218 + b2_omega_u * (v[i] * omega[j] + v[j] * omega[i])
219 + b2_omega_v * (u[i] * omega[j] + u[j] * omega[i])
220 + b2_u_v_c2_omega_v_omega_u * omega[i] * omega[j];
221 }
222 d_T_in_uv[j][j] += s_ident;
223 }
224 {
225 TP w[3];
226 const TP us = b[2] * omega_v;
227 const TP vs = b[2] * omega_u;
228 const TP omega_s = b[2] * u_v + c[2] * omega_v_omega_u;
229 for (int i = 0; i != 3; ++i) { w[i] = us * u[i] + vs * v[i] + omega_s * omega[i]; }
230 add_skew_matrix(w, d_T_in_uv);
231 }
232 }
233
234 void get_d_rotator_d_omega_in_u_d_u_in_a(const TP va[3], TP m[3][3]) {
235 compute_a(1, 3);
236 compute_b(0, 3);
237 const TP omega_va = dot_3(omega, va);
238 const TP s_ident = a[2] * omega_va;
239 const TP b2_omega_va = b[2] * omega_va;
240 TP omega_cross_va[3];
241 outer_product_3(omega, va, omega_cross_va);
242 for (int j = 0; j != 3; ++j) {
243 for (int i = 0; i != 3; ++i) {
244 m[j][i] = a[2] * omega[i] * va[j] + b[0] * va[i] * omega[j]
245 + b[1] * omega_cross_va[i] * omega[j] + b2_omega_va * omega[i] * omega[j];
246 }
247 m[j][j] += s_ident;
248 }
249 const TP w[] = {-va[0] * a[1], -va[1] * a[1], -va[2] * a[1]};
250 add_skew_matrix(w, m);
251 }
252
253 void get_d_T_d_omega_in_u_d_u_in_a(const TP va[3], TP m[3][3]) {
254 compute_a(2, 4);
255 compute_b(1, 4);
256 const TP omega_va = dot_3(omega, va);
257 const TP s_ident = a[3] * omega_va;
258 const TP b2_omega_va = b[3] * omega_va;
259 TP omega_cross_va[3];
260 outer_product_3(omega, va, omega_cross_va);
261 for (int j = 0; j != 3; ++j) {
262 for (int i = 0; i != 3; ++i) {
263 m[j][i] = a[3] * omega[i] * va[j] + b[1] * va[i] * omega[j]
264 + b[2] * omega_cross_va[i] * omega[j] + b2_omega_va * omega[i] * omega[j];
265 }
266 m[j][j] += s_ident;
267 }
268 const TP w[] = {-va[0] * a[2], -va[1] * a[2], -va[2] * a[2]};
269 add_skew_matrix(w, m);
270 }
271
272 void get_d_trans_rotator_d_omega_in_u_d_u_in_a(const TP va[3], TP m[3][3]) {
273 compute_a(1, 3);
274 compute_b(0, 3);
275 const TP omega_va = dot_3(omega, va);
276 const TP s_ident = a[2] * omega_va;
277 const TP b2_omega_va = b[2] * omega_va;
278 TP omega_cross_va[3];
279 outer_product_3(omega, va, omega_cross_va);
280 for (int j = 0; j != 3; ++j) {
281 for (int i = 0; i != 3; ++i) {
282 m[j][i] = a[2] * omega[i] * va[j] + b[0] * va[i] * omega[j]
283 - b[1] * omega_cross_va[i] * omega[j] + b2_omega_va * omega[i] * omega[j];
284 }
285 m[j][j] += s_ident;
286 }
287 const TP w[] = {va[0] * a[1], va[1] * a[1], va[2] * a[1]};
288 add_skew_matrix(w, m);
289 }
290
291 void get_d_trans_T_d_omega_in_u_d_u_in_a(const TP va[3], TP m[3][3]) {
292 compute_a(2, 4);
293 compute_b(1, 4);
294 const TP omega_va = dot_3(omega, va);
295 const TP s_ident = a[3] * omega_va;
296 const TP b2_omega_va = b[3] * omega_va;
297 TP omega_cross_va[3];
298 outer_product_3(omega, va, omega_cross_va);
299 for (int j = 0; j != 3; ++j) {
300 for (int i = 0; i != 3; ++i) {
301 m[j][i] = a[3] * omega[i] * va[j] + b[1] * va[i] * omega[j]
302 - b[2] * omega_cross_va[i] * omega[j] + b2_omega_va * omega[i] * omega[j];
303 }
304 m[j][j] += s_ident;
305 }
306 const TP w[] = {va[0] * a[2], va[1] * a[2], va[2] * a[2]};
307 add_skew_matrix(w, m);
308 }
309
310 void get_d2_rotator_d2_omega_in_uv_d_uv_in_ab(const TP va[3], const TP vb[3], TP m[3][3]) {
311 compute_a(2, 3);
312 compute_b(0, 3);
313 compute_c(0, 3);
314 TP va_cross_vb[3];
315 outer_product_3(va, vb, va_cross_vb);
316 TP omega_cross_va[3];
317 outer_product_3(omega, va, omega_cross_va);
318 const TP omega_va = dot_3(omega, va);
319 const TP omega_vb = dot_3(omega, vb);
320 const TP va_vb = dot_3(va, vb);
321 const TP omega_cross_va_vb = dot_3(omega_cross_va, vb);
322 const TP omega_va_omega_vb = omega_va * omega_vb;
323 const TP s_ident = b[0] * va_vb + b[1] * omega_cross_va_vb + b[2] * omega_va_omega_vb;
324 const TP s_omega_omega = c[0] * va_vb + c[1] * omega_cross_va_vb + c[2] * omega_va_omega_vb;
325 const TP b2_omega_va = b[2] * omega_va;
326 const TP b2_omega_vb = b[2] * omega_vb;
327 for (int j = 0; j != 3; ++j) {
328 for (int i = 0; i != 3; ++i) {
329 m[j][i] = a[2] * (va[i] * vb[j] + vb[i] * va[j])
330 + b[1] * (va_cross_vb[i] * omega[j] + omega[i] * va_cross_vb[j])
331 + b2_omega_va * (vb[i] * omega[j] + omega[i] * vb[j])
332 + b2_omega_vb * (va[i] * omega[j] + omega[i] * va[j])
333 + s_omega_omega * omega[i] * omega[j];
334 }
335 m[j][j] += s_ident;
336 }
337 }
338
339 void get_d2_T_d2_omega_in_uv_d_uv_in_ab(const TP va[3], const TP vb[3], TP m[3][3]) {
340 compute_a(3, 4);
341 compute_b(1, 4);
342 compute_c(1, 4);
343 TP va_cross_vb[3];
344 outer_product_3(va, vb, va_cross_vb);
345 TP omega_cross_va[3];
346 outer_product_3(omega, va, omega_cross_va);
347 const TP omega_va = dot_3(omega, va);
348 const TP omega_vb = dot_3(omega, vb);
349 const TP va_vb = dot_3(va, vb);
350 const TP omega_cross_va_vb = dot_3(omega_cross_va, vb);
351 const TP omega_va_omega_vb = omega_va * omega_vb;
352 const TP s_ident = b[1] * va_vb + b[2] * omega_cross_va_vb + b[3] * omega_va_omega_vb;
353 const TP s_omega_omega = c[1] * va_vb + c[2] * omega_cross_va_vb + c[3] * omega_va_omega_vb;
354 const TP b2_omega_va = b[3] * omega_va;
355 const TP b2_omega_vb = b[3] * omega_vb;
356 for (int i = 0; i != 3; ++i) {
357 for (int j = 0; j != 3; ++j) {
358 m[i][j] = a[3] * (va[i] * vb[j] + vb[i] * va[j])
359 + b[2] * (va_cross_vb[i] * omega[j] + omega[i] * va_cross_vb[j])
360 + b2_omega_va * (vb[i] * omega[j] + omega[i] * vb[j])
361 + b2_omega_vb * (va[i] * omega[j] + omega[i] * va[j])
362 + s_omega_omega * omega[i] * omega[j];
363 }
364 m[i][i] += s_ident;
365 }
366 }
367
368private:
369 static TP zero_if_nan(const TP v) {
370 if (v == v) { return v; }
371 return 0;
372 }
373
374 void compute_a(int min, int max) {
375 if (min < adef_min) {
376 for (int i = min; i < adef_min; ++i) { a[i] = get_a(i, nomega); }
377 adef_min = min;
378 }
379 if (max > adef_max) {
380 for (int i = std::max(min, adef_max); i < max; ++i) { a[i] = get_a(i, nomega); }
381 adef_max = max;
382 }
383 }
384
385 void compute_b(int min, int max) {
386 if (min < bdef_min) {
387 for (int i = min; i < bdef_min; ++i) { b[i] = get_b(i, nomega); }
388 bdef_min = min;
389 }
390 if (max > bdef_max) {
391 for (int i = std::max(min, bdef_max); i < max; ++i) { b[i] = get_b(i, nomega); }
392 bdef_max = max;
393 }
394 }
395
396 void compute_c(int min, int max) {
397 if (min < cdef_min) {
398 for (int i = min; i < cdef_min; ++i) { c[i] = get_c(i, nomega); }
399 cdef_min = min;
400 }
401 if (max > cdef_max) {
402 for (int i = std::max(min, cdef_max); i < max; ++i) { c[i] = get_c(i, nomega); }
403 cdef_max = max;
404 }
405 }
406
407public:
408 static TP get_a(const int i, const TP omega) {
409 assert(omega == omega);
410 if (omega >= min_for_trigo) {
411 switch (i) {
412 case 0:
413 return std::cos(omega);
414 case 1:
415 return std::sin(omega) / omega;
416 case 2:
417 return (1 - cos(omega)) / (omega * omega);
418 case 3:
419 return (omega - sin(omega)) / (omega * omega * omega);
420 }
421 }
422 {
423 const TP momega2 = -omega * omega;
424 assert(i >= 0 && i < 8);
425 TP t = TP(1) / factorial[i];
426 TP r = t;
427 int k2_i = i + 2;
428 while (1) {
429 t *= momega2 / ((k2_i - 1) * k2_i);
430 const TP new_r = r + t;
431 if (new_r == r) { return r; }
432 r = new_r;
433 k2_i += 2;
434 }
435 }
436 return 0;
437 }
438
439 static TP get_b(const int i, const TP omega) {
440 const TP omega2 = omega * omega;
441 if (omega >= min_for_trigo) {
442 switch (i) {
443 case 0:
444 return -sin(omega) / omega;
445 case 1:
446 return (omega * cos(omega) - sin(omega)) / (omega2 * omega);
447 case 2:
448 return (omega * sin(omega) - 2 + 2 * cos(omega)) / (omega2 * omega2);
449 case 3:
450 return (3 * sin(omega) - 2 * omega - omega * cos(omega))
451 / (omega2 * omega2 * omega);
452 }
453 }
454 {
455 assert(i >= 0 && 2 + i < 8);
456 const TP momega2 = -omega2;
457 int k2_2 = 4;
458 TP t = -TP(1) / factorial[2 + i];
459 TP r = 2 * t;
460 while (1) {
461 t *= momega2 / ((k2_2 + i - 1) * (k2_2 + i));
462 const TP new_r = r + k2_2 * t;
463 if (new_r == r) { return r; }
464 r = new_r;
465 k2_2 += 2;
466 }
467 }
468 return 0;
469 }
470
471 static TP get_c(const int i, const TP omega) {
472 const TP omega2 = omega * omega;
473 if (omega >= min_for_trigo) {
474 const TP omega4 = omega2 * omega2;
475 switch (i) {
476 case 0:
477 return (sin(omega) - omega * cos(omega)) / (omega2 * omega);
478 case 1:
479 return ((3 - omega2) * sin(omega) - 3 * omega * cos(omega)) / (omega4 * omega);
480 case 2:
481 return (8 + (omega2 - 8) * cos(omega) - 5 * omega * sin(omega))
482 / (omega4 * omega2);
483 case 3:
484 return (8 * omega + 7 * omega * cos(omega) + (omega2 - 15) * sin(omega))
485 / (omega4 * omega2 * omega);
486 }
487 }
488 {
489 const TP momega2 = -omega2;
490 int k = 1;
491 int i_4_2k = i + 6;
492 assert(i >= 0 && 4 + i < 8);
493 TP t = TP(1) / factorial[4 + i];
494 TP r = 8 * t;
495 while (1) {
496 t *= momega2 / ((i_4_2k - 1) * i_4_2k);
497 const TP new_r = r + 4 * (k + 1) * (k + 2) * t;
498 if (new_r == r) { return r; }
499 r = new_r;
500 k += 1;
501 i_4_2k += 2;
502 }
503 }
504 return 0;
505 }
506
507 static void add_skew_matrix(const TP w[3], TP r[3][3]) {
508 r[0][1] += w[2];
509 r[0][2] -= w[1];
510 r[1][0] -= w[2];
511 r[1][2] += w[0];
512 r[2][0] += w[1];
513 r[2][1] -= w[0];
514 }
515
516 static const TP min_for_trigo;
517
518 static constexpr int factorial[8] = {1, 1, 2, 6, 24, 120, 720, 5040};
519
520 const TP* omega;
521 TP nomega;
522 TP a[4];
523 TP b[4];
524 TP c[4];
525 int adef_min;
526 int adef_max;
527 int bdef_min;
528 int bdef_max;
529 int cdef_min;
530 int cdef_max;
531};
532
533template <typename TP>
534const TP FiniteRotation<TP>::min_for_trigo = TP(1);
535
536template <typename TP>
537constexpr int FiniteRotation<TP>::factorial[8];
538
544template <typename TP>
545inline void euler_angles_to_rotation_matrix(const TP e[3], TP rotator[3][3]) {
546 const TP c0 = cos(e[0]);
547 const TP c1 = cos(e[1]);
548 const TP c2 = cos(e[2]);
549 const TP s0 = sin(e[0]);
550 const TP s1 = sin(e[1]);
551 const TP s2 = sin(e[2]);
552 rotator[0][0] = c1 * c2;
553 rotator[0][1] = c1 * s2;
554 rotator[0][2] = -s1;
555 rotator[1][0] = -c0 * s2 + s0 * s1 * c2;
556 rotator[1][1] = c0 * c2 + s0 * s1 * s2;
557 rotator[1][2] = s0 * c1;
558 rotator[2][0] = s0 * s2 + c0 * s1 * c2;
559 rotator[2][1] = -s0 * c2 + c0 * s1 * s2;
560 rotator[2][2] = c0 * c1;
561}
562
568template <typename TP>
569inline void rotator_to_euler_angle(const TP rotator[3][3], TP e[3]) {
570 if (abs(rotator[0][2]) < 1 - 1e-15) {
571 e[1] = -asin(rotator[0][2]);
572 const TP inv_ce1 = TP(1) / cos(e[1]);
573 e[0] = atan2(rotator[1][2] * inv_ce1, rotator[2][2] * inv_ce1);
574 e[2] = atan2(rotator[0][1] * inv_ce1, rotator[0][0] * inv_ce1);
575 } else {
576 e[2] = 0;
577 if (rotator[0][2] < 0) {
578 e[1] = M_PIl * 0.5;
579 e[0] = atan2(rotator[1][0], rotator[2][0]);
580 } else {
581 e[1] = -M_PIl * 0.5;
582 e[0] = atan2(-rotator[1][0], -rotator[2][0]);
583 }
584 }
585}
586
587template <typename TP>
588inline void rotator_to_quaternion(const TP rotator[3][3], TP q[4]) {
589 if (rotator[1][1] > -rotator[2][2] && rotator[0][0] > -rotator[1][1]
590 && rotator[0][0] > -rotator[2][2]) {
591 const TP s = 0.5 * sqrt(1.0 + rotator[0][0] + rotator[1][1] + rotator[2][2]);
592 const TP invs = 0.25 / s;
593 q[0] = s;
594 q[1] = (rotator[2][1] - rotator[1][2]) * invs;
595 q[2] = (rotator[0][2] - rotator[2][0]) * invs;
596 q[3] = (rotator[1][0] - rotator[0][1]) * invs;
597 } else if (
598 rotator[1][1] < -rotator[2][2] && rotator[0][0] > rotator[1][1]
599 && rotator[0][0] > rotator[2][2]) {
600 const TP s = 0.5 * sqrt(1.0 + rotator[0][0] - rotator[1][1] - rotator[2][2]);
601 const TP invs = 0.25 / s;
602 q[0] = (rotator[2][1] - rotator[1][2]) * invs;
603 q[1] = s;
604 q[2] = (rotator[1][0] + rotator[0][1]) * invs;
605 q[3] = (rotator[0][2] + rotator[2][0]) * invs;
606 } else if (
607 rotator[1][1] > rotator[2][2] && rotator[0][0] < rotator[1][1]
608 && rotator[0][0] < -rotator[2][2]) {
609 const TP s = 0.5 * sqrt(1.0 - rotator[0][0] + rotator[1][1] - rotator[2][2]);
610 const TP invs = 0.25 / s;
611 q[0] = (rotator[0][2] - rotator[2][0]) * invs;
612 q[1] = (rotator[1][0] + rotator[0][1]) * invs;
613 q[2] = s;
614 q[3] = (rotator[2][1] + rotator[1][2]) * invs;
615 } else if (
616 rotator[1][1] < rotator[2][2] && rotator[0][0] < -rotator[1][1]
617 && rotator[0][0] < rotator[2][2]) {
618 const TP s = 0.5 * sqrt(1.0 - rotator[0][0] - rotator[1][1] + rotator[2][2]);
619 const TP invs = 0.25 / s;
620 q[0] = (rotator[1][0] - rotator[0][1]) * invs;
621 q[1] = (rotator[2][0] + rotator[0][2]) * invs;
622 q[2] = (rotator[2][1] + rotator[1][2]) * invs;
623 q[3] = s;
624 } else {
625 assert(0);
626 }
627}
628
629template <typename TP>
630inline void rotation_vector_to_quaternion(
631 const TP rotation_vector[3], TP quaternion[4], TP d_quaternion_d_rotation_vector[3][4] = 0) {
632 const TP omega = sqrt(dot_3(rotation_vector, rotation_vector));
633 const TP omega_2 = omega / 2;
634 const TP c_v2 = cos(omega_2);
635 const TP s_omega_2_omega_2 = FiniteRotation<TP>::get_a(1, omega_2);
636 if (quaternion) {
637 quaternion[0] = c_v2;
638 const TP s = s_omega_2_omega_2 * 0.5;
639 for (int i = 0; i != 3; ++i) { quaternion[i + 1] = rotation_vector[i] * s; }
640 }
641 if (d_quaternion_d_rotation_vector) {
642 const TP a = s_omega_2_omega_2 / 4;
643 const TP b = omega < std::numeric_limits<TP>::epsilon()
644 ? TP(0)
645 : (c_v2 * omega - 2 * sin(omega_2)) / (2 * omega * omega * omega);
646 for (int j = 0; j != 3; ++j) {
647 d_quaternion_d_rotation_vector[j][0] = -a * rotation_vector[j];
648 for (int i = 0; i != 3; ++i) {
649 d_quaternion_d_rotation_vector[j][i + 1] =
650 b * rotation_vector[i] * rotation_vector[j];
651 }
652 d_quaternion_d_rotation_vector[j][j + 1] += 2 * a;
653 }
654 }
655}
656
657template <typename TP>
658inline void quaternion_to_rotation_vector(
659 const TP quaternion[4], TP rotation_vector[3], TP d_rotator_vector_d_quaternion[4][3] = 0) {
660 const TP n2 = 1 - quaternion[0] * quaternion[0];
661 const TP n = sqrt(n2);
662 const TP s = n < std::numeric_limits<TP>::epsilon() ? TP(2) : TP(2) * acos(quaternion[0]) / n;
663 if (rotation_vector) {
664 for (int i = 0; i != 3; ++i) { rotation_vector[i] = s * quaternion[i + 1]; }
665 }
666 if (d_rotator_vector_d_quaternion) {
667 const TP c = n < std::numeric_limits<TP>::epsilon() ? TP(0) : TP(1) / n2;
668 for (int i = 0; i != 3; ++i) {
669 d_rotator_vector_d_quaternion[0][i] = c * quaternion[i + 1] * (s * quaternion[0] - 2);
670 for (int j = 1; j != 4; ++j) { d_rotator_vector_d_quaternion[j][i] = 0; }
671 d_rotator_vector_d_quaternion[i + 1][i] = s;
672 }
673 }
674}
675
676template <typename TP>
677inline TP quaternion_norm(const TP quaternion[4]) {
678 return sqrt(quaternion[0] * quaternion[0] + dot_3(quaternion + 1, quaternion + 1));
679}
680
681template <typename TP>
682inline void quaternion_renormalisation(TP q[4]) {
683 const TP s = TP(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
684 for (int i = 0; i != 4; ++i) { q[i] *= s; }
685}
686
687template <typename TP>
688inline void quaternion_to_rotator(const TP quaternion[4], TP rotator[3][3]) {
689 const TP s = 2 / quaternion_norm(quaternion);
690 for (int j = 0; j != 3; ++j) {
691 for (int i = 0; i != 3; ++i) { rotator[j][i] = s * quaternion[i + 1] * quaternion[j + 1]; }
692 }
693 const TP sq = s * quaternion[0];
694 const TP sq2_1 = sq * quaternion[0] - 1;
695 rotator[0][0] += sq2_1;
696 rotator[0][1] -= sq * quaternion[3];
697 rotator[0][2] += sq * quaternion[2];
698 rotator[1][0] += sq * quaternion[3];
699 rotator[1][1] += sq2_1;
700 rotator[1][2] -= sq * quaternion[1];
701 rotator[2][0] -= sq * quaternion[2];
702 rotator[2][1] += sq * quaternion[1];
703 rotator[2][2] += sq2_1;
704}
705
706template <typename TP>
707inline void quaternion_add(const TP qa[4], const TP qb[4], TP qc[4]) {
708 for (int i = 0; i != 4; ++i) { qc[i] = qa[i] + qb[i]; }
709}
710
711template <typename TP>
712inline TP quaternion_dot(const TP qa[4], const TP qb[4]) {
713 return qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3];
714}
715
716template <typename TP>
717inline void quaternion_product(const TP qa[4], const TP qb[4], TP qc[4]) {
718 qc[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
719 qc[1] = qa[0] * qb[1] + qa[1] * qb[0] - qa[2] * qb[3] + qa[3] * qb[2];
720 qc[2] = qa[0] * qb[2] + qa[1] * qb[3] + qa[2] * qb[0] - qa[3] * qb[1];
721 qc[3] = qa[0] * qb[3] - qa[1] * qb[2] + qa[2] * qb[1] + qa[3] * qb[0];
722}
723
724template <typename TP>
725inline void d_quaternion_product_d_qa(const TP qb[4], TP d_qc_d_qa[4][4]) {
726 d_qc_d_qa[0][0] = qb[0];
727 d_qc_d_qa[0][1] = qb[1];
728 d_qc_d_qa[0][2] = qb[2];
729 d_qc_d_qa[0][3] = qb[3];
730 d_qc_d_qa[1][0] = -qb[1];
731 d_qc_d_qa[1][1] = qb[0];
732 d_qc_d_qa[1][2] = qb[3];
733 d_qc_d_qa[1][3] = -qb[2];
734 d_qc_d_qa[2][0] = -qb[2];
735 d_qc_d_qa[2][1] = -qb[3];
736 d_qc_d_qa[2][2] = qb[0];
737 d_qc_d_qa[2][3] = qb[1];
738 d_qc_d_qa[3][0] = -qb[3];
739 d_qc_d_qa[3][1] = qb[2];
740 d_qc_d_qa[3][2] = -qb[1];
741 d_qc_d_qa[3][3] = qb[0];
742}
743
744template <typename TP>
745inline void d_quaternion_product_d_qb(const TP qa[4], TP d_qc_d_qb[4][4]) {
746 d_qc_d_qb[0][0] = qa[0];
747 d_qc_d_qb[0][1] = qa[1];
748 d_qc_d_qb[0][2] = qa[2];
749 d_qc_d_qb[0][3] = qa[3];
750 d_qc_d_qb[1][0] = -qa[1];
751 d_qc_d_qb[1][1] = qa[0];
752 d_qc_d_qb[1][2] = -qa[3];
753 d_qc_d_qb[1][3] = qa[2];
754 d_qc_d_qb[2][0] = -qa[2];
755 d_qc_d_qb[2][1] = qa[3];
756 d_qc_d_qb[2][2] = qa[0];
757 d_qc_d_qb[2][3] = -qa[1];
758 d_qc_d_qb[3][0] = -qa[3];
759 d_qc_d_qb[3][1] = -qa[2];
760 d_qc_d_qb[3][2] = qa[1];
761 d_qc_d_qb[3][3] = qa[0];
762}
763
764template <typename TP>
765inline void quaternion_inverse(const TP q[4], TP qinv[4]) {
766 const TP s = TP(1) / quaternion_norm(q);
767 qinv[0] = q[0] * s;
768 for (int i = 1; i != 4; ++i) { qinv[i] = -s * q[i]; }
769}
770
771} // namespace b2000
772
773#endif /* _B2FINITE_ROTATION_H_ */
b2000::details::acos
csda< T > acos(const csda< T > &z)
Arc cosine of a csda number.
Definition b2csda.H:431
b2000::details::sin
csda< T > sin(const csda< T > &a)
Sine of a csda number.
Definition b2csda.H:402
b2000::details::sqrt
csda< T > sqrt(const csda< T > &a)
Square root of a csda number.
Definition b2csda.H:396
b2000::details::cos
csda< T > cos(const csda< T > &a)
Cosine of a csda number.
Definition b2csda.H:410
b2tensor_calculus.H
b2000
Contains the base classes for implementing Finite Elements.
Definition b2boundary_condition.H:32
b2000::rotator_to_euler_angle
void rotator_to_euler_angle(const TP rotator[3][3], TP e[3])
Definition b2finite_rotation.H:569
b2000::outer_product_3
void outer_product_3(const T1 a[3], const T2 b[3], T3 c[3])
Definition b2tensor_calculus.H:389
b2000::euler_angles_to_rotation_matrix
void euler_angles_to_rotation_matrix(const TP e[3], TP rotator[3][3])
Definition b2finite_rotation.H:545
b2000::dot_3
T dot_3(const T a[3], const T b[3])
Definition b2tensor_calculus.H:328