1 #ifndef __CS_MATH_H__
2 #define __CS_MATH_H__
3
4 /*============================================================================
5 * Mathematical base functions.
6 *============================================================================*/
7
8 /*
9 This file is part of Code_Saturne, a general-purpose CFD tool.
10
11 Copyright (C) 1998-2021 EDF S.A.
12
13 This program is free software; you can redistribute it and/or modify it under
14 the terms of the GNU General Public License as published by the Free Software
15 Foundation; either version 2 of the License, or (at your option) any later
16 version.
17
18 This program is distributed in the hope that it will be useful, but WITHOUT
19 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
20 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
21 details.
22
23 You should have received a copy of the GNU General Public License along with
24 this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
25 Street, Fifth Floor, Boston, MA 02110-1301, USA.
26 */
27
28 /*----------------------------------------------------------------------------*/
29
30 #include "cs_defs.h"
31
32 /*----------------------------------------------------------------------------
33 * Standard C library headers
34 *----------------------------------------------------------------------------*/
35
36 #include <assert.h>
37 #include <math.h>
38
39 /*----------------------------------------------------------------------------
40 * Local headers
41 *----------------------------------------------------------------------------*/
42
43 #if defined(DEBUG) && !defined(NDEBUG) /* Sanity check */
44 #include "bft_error.h"
45 #endif
46
47 /*----------------------------------------------------------------------------*/
48
49 BEGIN_C_DECLS
50
51 /*=============================================================================
52 * Local Macro definitions
53 *============================================================================*/
54
55 /*============================================================================
56 * Type definition
57 *============================================================================*/
58
59 /* Symmetric tensor component name */
60
61 typedef enum {
62
63 XX,
64 YY,
65 ZZ,
66 XY,
67 YZ,
68 XZ
69
70 } cs_math_sym_tensor_component_t;
71
72 /*============================================================================
73 * Global variables
74 *============================================================================*/
75
76 /* Numerical constants */
77
78 extern const cs_real_t cs_math_zero_threshold;
79 extern const cs_real_t cs_math_1ov3;
80 extern const cs_real_t cs_math_2ov3;
81 extern const cs_real_t cs_math_4ov3;
82 extern const cs_real_t cs_math_5ov3;
83 extern const cs_real_t cs_math_1ov6;
84 extern const cs_real_t cs_math_1ov12;
85 extern const cs_real_t cs_math_1ov24;
86 extern const cs_real_t cs_math_epzero;
87 extern const cs_real_t cs_math_infinite_r;
88 extern const cs_real_t cs_math_big_r;
89 extern const cs_real_t cs_math_pi;
90
91 /*=============================================================================
92 * Inline static function prototypes
93 *============================================================================*/
94
95 /*----------------------------------------------------------------------------*/
96 /*!
97 * \brief Computes the binomial coefficient of n and k
98 *
99 * \param[in] n first argument
100 * \param[in] k second argument
101 *
102 * \return the value of binom(n)(k)
103 */
104 /*----------------------------------------------------------------------------*/
105
106 static inline int
cs_math_binom(int n,int k)107 cs_math_binom(int n,
108 int k)
109 {
110 int ret = 1;
111 assert(n >= k);
112
113 const int n_iter = (k > n-k) ? n-k : k;
114 for (int j = 1; j <= n_iter; j++, n--) {
115 if (n % j == 0)
116 ret *= n/j;
117 else if (ret % j == 0)
118 ret = ret/j*n;
119 else
120 ret = (ret*n)/j;
121 }
122
123 return ret;
124 }
125
126 /*----------------------------------------------------------------------------*/
127 /*!
128 * \brief Compute the absolute value of a real value.
129 *
130 * \param[in] x value
131 *
132 * \return the real of the given value
133 */
134 /*----------------------------------------------------------------------------*/
135
136 static inline cs_real_t
cs_math_fabs(cs_real_t x)137 cs_math_fabs(cs_real_t x)
138 {
139 cs_real_t ret = (x < 0) ? -x : x;
140
141 return ret;
142 }
143
144 /*----------------------------------------------------------------------------*/
145 /*!
146 * \brief Compute the min value of two real values.
147 *
148 * \param[in] x, y values
149 *
150 * \return the min value
151 */
152 /*----------------------------------------------------------------------------*/
153
154 static inline cs_real_t
cs_math_fmin(cs_real_t x,cs_real_t y)155 cs_math_fmin(cs_real_t x,
156 cs_real_t y)
157 {
158 cs_real_t ret = (x < y) ? x : y;
159
160 return ret;
161 }
162
163 /*----------------------------------------------------------------------------*/
164 /*!
165 * \brief Compute the max value of two real values.
166 *
167 * \param[in] x, y values
168 *
169 * \return the max value
170 */
171 /*----------------------------------------------------------------------------*/
172
173 static inline cs_real_t
cs_math_fmax(cs_real_t x,cs_real_t y)174 cs_math_fmax(cs_real_t x,
175 cs_real_t y)
176 {
177 cs_real_t ret = (x < y) ? y : x;
178
179 return ret;
180 }
181
182 /*----------------------------------------------------------------------------*/
183 /*!
184 * \brief Compute the square of a real value.
185 *
186 * \param[in] x value
187 *
188 * \return the square of the given value
189 */
190 /*----------------------------------------------------------------------------*/
191
192 static inline cs_real_t
cs_math_sq(cs_real_t x)193 cs_math_sq(cs_real_t x)
194 {
195 return x*x;
196 }
197
198 /*----------------------------------------------------------------------------*/
199 /*!
200 * \brief Compute the square of a real value.
201 *
202 * \param[in] x value
203 *
204 * \return the square of the given value
205 */
206 /*----------------------------------------------------------------------------*/
207
208 static inline cs_real_t
cs_math_pow2(cs_real_t x)209 cs_math_pow2(cs_real_t x)
210 {
211 return x*x;
212 }
213
214 /*----------------------------------------------------------------------------*/
215 /*!
216 * \brief Compute the cube of a real value.
217 *
218 * \param[in] x value
219 *
220 * \return the cube of the given value
221 */
222 /*----------------------------------------------------------------------------*/
223
224 static inline cs_real_t
cs_math_pow3(cs_real_t x)225 cs_math_pow3(cs_real_t x)
226 {
227 return x*x*x;
228 }
229
230 /*----------------------------------------------------------------------------*/
231 /*!
232 * \brief Compute the 4-th power of a real value.
233 *
234 * \param[in] x value
235 *
236 * \return the 4th power of the given value
237 */
238 /*----------------------------------------------------------------------------*/
239
240 static inline cs_real_t
cs_math_pow4(cs_real_t x)241 cs_math_pow4(cs_real_t x)
242 {
243 return (x*x)*(x*x);
244 }
245
246 /*----------------------------------------------------------------------------*/
247 /*!
248 * \brief Compute the (euclidean) distance between two points xa and xb in
249 * a Cartesian coordinate system of dimension 3.
250 *
251 * \param[in] xa first coordinate
252 * \param[in] xb second coordinate
253 *
254 * \return the length between two points xa and xb
255 */
256 /*----------------------------------------------------------------------------*/
257
258 static inline cs_real_t
cs_math_3_distance(const cs_real_t xa[3],const cs_real_t xb[3])259 cs_math_3_distance(const cs_real_t xa[3],
260 const cs_real_t xb[3])
261 {
262 cs_real_t v[3];
263
264 v[0] = xb[0] - xa[0];
265 v[1] = xb[1] - xa[1];
266 v[2] = xb[2] - xa[2];
267
268 return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
269 }
270
271 /*----------------------------------------------------------------------------*/
272 /*!
273 * \brief Compute \f$ (\vect{x}_b - \vect{x}_a) \cdot \vect{x}_c \f$
274 *
275 * \param[in] xa first coordinate
276 * \param[in] xb second coordinate
277 * \param[in] xc third coordinate
278 *
279 * \return the dot product
280 */
281 /*----------------------------------------------------------------------------*/
282
283 static inline cs_real_t
cs_math_3_distance_dot_product(const cs_real_t xa[3],const cs_real_t xb[3],const cs_real_t xc[3])284 cs_math_3_distance_dot_product(const cs_real_t xa[3],
285 const cs_real_t xb[3],
286 const cs_real_t xc[3])
287 {
288 return ((xb[0] - xa[0])*xc[0]+(xb[1] - xa[1])*xc[1]+(xb[2] - xa[2])*xc[2]);
289 }
290
291 /*----------------------------------------------------------------------------*/
292 /*!
293 * \brief Compute the squared distance between two points xa and xb in
294 * a Cartesian coordinate system of dimension 3.
295 *
296 * \param[in] xa first coordinate
297 * \param[in] xb second coordinate
298 *
299 * \return the square length between two points xa and xb
300 */
301 /*----------------------------------------------------------------------------*/
302
303 static inline cs_real_t
cs_math_3_square_distance(const cs_real_t xa[3],const cs_real_t xb[3])304 cs_math_3_square_distance(const cs_real_t xa[3],
305 const cs_real_t xb[3])
306 {
307 cs_real_t v[3] = {xb[0] - xa[0],
308 xb[1] - xa[1],
309 xb[2] - xa[2]};
310
311 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
312 }
313
314 /*----------------------------------------------------------------------------*/
315 /*!
316 * \brief Compute the dot product of two vectors of 3 real values.
317 *
318 * \param[in] u vector of 3 real values
319 * \param[in] v vector of 3 real values
320 *
321 * \return the resulting dot product u.v.
322 */
323 /*----------------------------------------------------------------------------*/
324
325 static inline cs_real_t
cs_math_3_dot_product(const cs_real_t u[3],const cs_real_t v[3])326 cs_math_3_dot_product(const cs_real_t u[3],
327 const cs_real_t v[3])
328 {
329 cs_real_t uv = u[0]*v[0] + u[1]*v[1] + u[2]*v[2];
330
331 return uv;
332 }
333
334 /*----------------------------------------------------------------------------*/
335 /*!
336 * \brief Compute the dot product of a tensor t with two vectors, n1 and n2.
337 *
338 * \param[in] n1 vector of 3 real values
339 * \param[in] t tensor of 3x3 real values
340 * \param[in] n2 vector of 3 real values
341 *
342 * \return the resulting dot product n1.t.n2.
343 */
344 /*----------------------------------------------------------------------------*/
345
346 static inline cs_real_t
cs_math_3_33_3_dot_product(const cs_real_t n1[3],const cs_real_t t[3][3],const cs_real_t n2[3])347 cs_math_3_33_3_dot_product(const cs_real_t n1[3],
348 const cs_real_t t[3][3],
349 const cs_real_t n2[3])
350 {
351 cs_real_t n_t_n
352 = ( n1[0]*t[0][0]*n2[0] + n1[1]*t[1][0]*n2[0] + n1[2]*t[2][0]*n2[0]
353 + n1[0]*t[0][1]*n2[1] + n1[1]*t[1][1]*n2[1] + n1[2]*t[2][1]*n2[1]
354 + n1[0]*t[0][2]*n2[2] + n1[1]*t[1][2]*n2[2] + n1[2]*t[2][2]*n2[2]);
355 return n_t_n;
356 }
357
358 /*----------------------------------------------------------------------------*/
359 /*!
360 * \brief Compute the dot product of a symmetric tensor t with two vectors,
361 * n1 and n2.
362 *
363 * \param[in] n1 vector of 3 real values
364 * \param[in] t tensor of 6 real values
365 * [ 0 3 5 ]
366 * [ 3 1 4 ]
367 * [ 5 4 2 ]
368 * \param[in] n2 vector of 3 real values
369 *
370 * \return the resulting dot product n1.t.n2.
371 */
372 /*----------------------------------------------------------------------------*/
373
374 static inline cs_real_t
cs_math_3_sym_33_3_dot_product(const cs_real_t n1[3],const cs_real_t t[6],const cs_real_t n2[3])375 cs_math_3_sym_33_3_dot_product(const cs_real_t n1[3],
376 const cs_real_t t[6],
377 const cs_real_t n2[3])
378 {
379 cs_real_t n_t_n
380 = ( n1[0]*t[0]*n2[0] + n1[1]*t[3]*n2[0] + n1[2]*t[5]*n2[0]
381 + n1[0]*t[3]*n2[1] + n1[1]*t[1]*n2[1] + n1[2]*t[4]*n2[1]
382 + n1[0]*t[5]*n2[2] + n1[1]*t[4]*n2[2] + n1[2]*t[2]*n2[2]);
383 return n_t_n;
384 }
385
386 /*----------------------------------------------------------------------------*/
387 /*!
388 * \brief Compute the euclidean norm of a vector of dimension 3
389 *
390 * \param[in] v
391 *
392 * \return the value of the norm
393 */
394 /*----------------------------------------------------------------------------*/
395
396 static inline cs_real_t
cs_math_3_norm(const cs_real_t v[3])397 cs_math_3_norm(const cs_real_t v[3])
398 {
399 return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
400 }
401
402 /*----------------------------------------------------------------------------*/
403 /*!
404 * \brief Compute the square norm of a vector of 3 real values.
405 *
406 * \param[in] v vector of 3 real values
407 *
408 * \return square norm of v.
409 */
410 /*----------------------------------------------------------------------------*/
411
412 static inline cs_real_t
cs_math_3_square_norm(const cs_real_t v[3])413 cs_math_3_square_norm(const cs_real_t v[3])
414 {
415 cs_real_t v2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
416
417 return v2;
418 }
419
420 /*----------------------------------------------------------------------------*/
421 /*!
422 * \brief Normalize a vector of 3 real values.
423 *
424 * \deprecated: use \ref cs_math_3_normalize instead.
425 *
426 * \param[in] vin vector
427 * \param[out] vout normalized vector
428 */
429 /*----------------------------------------------------------------------------*/
430
431 static inline void
cs_math_3_normalise(const cs_real_t vin[3],cs_real_t vout[3])432 cs_math_3_normalise(const cs_real_t vin[3],
433 cs_real_t vout[3])
434 {
435 cs_real_t norm = cs_math_3_norm(vin);
436
437 cs_real_t inv_norm = ((norm > cs_math_zero_threshold) ? 1. / norm : 0);
438
439 vout[0] = inv_norm * vin[0];
440 vout[1] = inv_norm * vin[1];
441 vout[2] = inv_norm * vin[2];
442 }
443
444 /*----------------------------------------------------------------------------*/
445 /*!
446 * \brief Normalise a vector of 3 real values.
447 *
448 * To normalize in-place, \p vin and \p vout may point to the same array.
449 *
450 * \param[in] vin vector
451 * \param[out] vout normalized vector
452 */
453 /*----------------------------------------------------------------------------*/
454
455 static inline void
cs_math_3_normalize(const cs_real_t vin[3],cs_real_t vout[3])456 cs_math_3_normalize(const cs_real_t vin[3],
457 cs_real_t vout[3])
458 {
459 cs_real_t norm = cs_math_3_norm(vin);
460
461 cs_real_t inv_norm = ((norm > cs_math_zero_threshold) ? 1. / norm : 0);
462
463 vout[0] = inv_norm * vin[0];
464 vout[1] = inv_norm * vin[1];
465 vout[2] = inv_norm * vin[2];
466 }
467
468 /*----------------------------------------------------------------------------*/
469 /*!
470 * \brief Orthogonal projection of a vector with respect to a normalised
471 * vector.
472 *
473 * \param[in] n normal vector direction
474 * \param[in] v vector to be projected
475 * \param[out] vout projection
476 */
477 /*----------------------------------------------------------------------------*/
478
479 static inline void
cs_math_3_orthogonal_projection(const cs_real_t n[3],const cs_real_t v[3],cs_real_t vout[restrict3])480 cs_math_3_orthogonal_projection(const cs_real_t n[3],
481 const cs_real_t v[3],
482 cs_real_t vout[restrict 3])
483 {
484 vout[0] = v[0]*(1.-n[0]*n[0])- v[1]* n[1]*n[0] - v[2]* n[2]*n[0];
485 vout[1] = -v[0]* n[0]*n[1] + v[1]*(1.-n[1]*n[1])- v[2]* n[2]*n[1];
486 vout[2] = -v[0]* n[0]*n[2] - v[1]* n[1]*n[2] + v[2]*(1.-n[2]*n[2]);
487 }
488
489 /*----------------------------------------------------------------------------*/
490 /*!
491 * \brief Add the dot product with a normal vector to the normal direction
492 * to a vector.
493 *
494 * \param[in] n normalised face normal vector
495 * \param[in] factor factor
496 * \param[in, out] v vector to be scaled
497 */
498 /*----------------------------------------------------------------------------*/
499
500 static inline void
cs_math_3_normal_scaling(const cs_real_t n[3],cs_real_t factor,cs_real_t v[3])501 cs_math_3_normal_scaling(const cs_real_t n[3],
502 cs_real_t factor,
503 cs_real_t v[3])
504 {
505 cs_real_t v_dot_n = (factor -1.) * cs_math_3_dot_product(v, n);
506 for (int i = 0; i < 3; i++)
507 v[i] += v_dot_n * n[i];
508 }
509
510 /*----------------------------------------------------------------------------*/
511 /*!
512 * \brief Add the dot product with a normal vector to the normal,normal
513 * component of a tensor:
514 * t += factor * n.t.n n(x)n
515 *
516 * \param[in] n normalised face normal vector
517 * \param[in] factor factor
518 * \param[in,out] t matrix to be scaled
519 */
520 /*----------------------------------------------------------------------------*/
521
522 static inline void
cs_math_33_normal_scaling_add(const cs_real_t n[3],cs_real_t factor,cs_real_t t[3][3])523 cs_math_33_normal_scaling_add(const cs_real_t n[3],
524 cs_real_t factor,
525 cs_real_t t[3][3])
526 {
527 cs_real_t n_t_n = (factor -1.) *
528 ( n[0] * t[0][0] * n[0] + n[1] * t[1][0] * n[0] + n[2] * t[2][0] * n[0]
529 + n[0] * t[0][1] * n[1] + n[1] * t[1][1] * n[1] + n[2] * t[2][1] * n[1]
530 + n[0] * t[0][2] * n[2] + n[1] * t[1][2] * n[2] + n[2] * t[2][2] * n[2]);
531 for (int i = 0; i < 3; i++) {
532 for (int j = 0; j < 3; j++)
533 t[i][j] += n_t_n * n[i] * n[j];
534 }
535 }
536 /*----------------------------------------------------------------------------*/
537 /*!
538 * \brief Compute the product of a matrix of 3x3 real values by a vector of 3
539 * real values.
540 *
541 * \param[in] m matrix of 3x3 real values
542 * \param[in] v vector of 3 real values
543 * \param[out] mv vector of 3 real values
544 */
545 /*----------------------------------------------------------------------------*/
546
547 static inline void
cs_math_33_3_product(const cs_real_t m[3][3],const cs_real_t v[3],cs_real_t mv[restrict3])548 cs_math_33_3_product(const cs_real_t m[3][3],
549 const cs_real_t v[3],
550 cs_real_t mv[restrict 3])
551 {
552 mv[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
553 mv[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
554 mv[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
555 }
556
557 /*----------------------------------------------------------------------------*/
558 /*!
559 * \brief Compute the product of a matrix of 3x3 real values by a vector of 3
560 * real values add.
561 *
562 * \param[in] m matrix of 3x3 real values
563 * \param[in] v vector of 3 real values
564 * \param[in,out] mv vector of 3 real values
565 */
566 /*----------------------------------------------------------------------------*/
567
568 static inline void
cs_math_33_3_product_add(const cs_real_t m[3][3],const cs_real_t v[3],cs_real_t mv[restrict3])569 cs_math_33_3_product_add(const cs_real_t m[3][3],
570 const cs_real_t v[3],
571 cs_real_t mv[restrict 3])
572 {
573 mv[0] += m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
574 mv[1] += m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
575 mv[2] += m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
576 }
577
578 /*----------------------------------------------------------------------------*/
579 /*!
580 * \brief Compute the product of the transpose of a matrix of 3x3 real
581 * values by a vector of 3 real values.
582 *
583 * \param[in] m matrix of 3x3 real values
584 * \param[in] v vector of 3 real values
585 * \param[out] mv vector of 3 real values
586 */
587 /*----------------------------------------------------------------------------*/
588
589 static inline void
cs_math_33t_3_product(const cs_real_t m[3][3],const cs_real_t v[3],cs_real_t mv[restrict3])590 cs_math_33t_3_product(const cs_real_t m[3][3],
591 const cs_real_t v[3],
592 cs_real_t mv[restrict 3])
593 {
594 mv[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
595 mv[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
596 mv[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
597 }
598
599 /*----------------------------------------------------------------------------*/
600 /*!
601 * \brief Compute the product of a symmetric matrix of 3x3 real values by
602 * a vector of 3 real values.
603 * NB: Symmetric matrix are stored as follows (s11, s22, s33, s12, s23, s13)
604 *
605 * \param[in] m matrix of 3x3 real values
606 * \param[in] v vector of 3 real values
607 * \param[out] mv vector of 3 real values
608 */
609 /*----------------------------------------------------------------------------*/
610
611 static inline void
cs_math_sym_33_3_product(const cs_real_t m[6],const cs_real_t v[3],cs_real_t mv[restrict3])612 cs_math_sym_33_3_product(const cs_real_t m[6],
613 const cs_real_t v[3],
614 cs_real_t mv[restrict 3])
615 {
616 mv[0] = m[0] * v[0] + m[3] * v[1] + m[5] * v[2];
617 mv[1] = m[3] * v[0] + m[1] * v[1] + m[4] * v[2];
618 mv[2] = m[5] * v[0] + m[4] * v[1] + m[2] * v[2];
619 }
620
621 /*----------------------------------------------------------------------------*/
622 /*!
623 * \brief Compute the product of a symmetric matrix of 3x3 real values by
624 * a vector of 3 real values and add it to the vector.
625 * NB: Symmetric matrix are stored as follows (s11, s22, s33, s12, s23, s13)
626 *
627 * \param[in] m matrix of 3x3 real values
628 * \param[in] v vector of 3 real values
629 * \param[out] mv vector of 3 real values
630 */
631 /*----------------------------------------------------------------------------*/
632
633 static inline void
cs_math_sym_33_3_product_add(const cs_real_t m[6],const cs_real_t v[3],cs_real_t mv[restrict3])634 cs_math_sym_33_3_product_add(const cs_real_t m[6],
635 const cs_real_t v[3],
636 cs_real_t mv[restrict 3])
637 {
638 mv[0] += m[0] * v[0] + m[3] * v[1] + m[5] * v[2];
639 mv[1] += m[3] * v[0] + m[1] * v[1] + m[4] * v[2];
640 mv[2] += m[5] * v[0] + m[4] * v[1] + m[2] * v[2];
641 }
642
643 /*----------------------------------------------------------------------------*/
644 /*!
645 * \brief Compute the trace of a symmetric tensor.
646 *
647 * \param[in] t vector of 6 real values (symmetric tensor)
648 *
649 * \return trace (t[0] + t[1] + t[2])
650 */
651 /*----------------------------------------------------------------------------*/
652
653 static inline cs_real_t
cs_math_6_trace(const cs_real_t t[6])654 cs_math_6_trace(const cs_real_t t[6])
655 {
656 return (t[0] + t[1] + t[2]);
657 }
658
659 /*----------------------------------------------------------------------------*/
660 /*!
661 * \brief Compute the product of a matrix of 6x6 real values by
662 * a vector of 6 real values.
663 *
664 * \param[in] m matrix of 6x6 real values
665 * \param[in] v vector of 6 real values
666 * \param[out] mv vector of 6 real values
667 */
668 /*----------------------------------------------------------------------------*/
669
670 static inline void
cs_math_66_6_product(const cs_real_t m[6][6],const cs_real_t v[6],cs_real_t mv[restrict6])671 cs_math_66_6_product(const cs_real_t m[6][6],
672 const cs_real_t v[6],
673 cs_real_t mv[restrict 6])
674 {
675 for (int i = 0; i < 6; i++) {
676 for (int j = 0; j < 6; j++)
677 mv[i] = m[i][j] * v[j];
678 }
679 }
680
681 /*----------------------------------------------------------------------------*/
682 /*!
683 * \brief Compute the product of a matrix of 6x6 real values by
684 * a vector of 6 real values and add it to the vector.
685 *
686 * \param[in] m matrix of 6x6 real values
687 * \param[in] v vector of 6 real values
688 * \param[out] mv vector of 6 real values
689 */
690 /*----------------------------------------------------------------------------*/
691
692 static inline void
cs_math_66_6_product_add(const cs_real_t m[6][6],const cs_real_t v[6],cs_real_t mv[restrict6])693 cs_math_66_6_product_add(const cs_real_t m[6][6],
694 const cs_real_t v[6],
695 cs_real_t mv[restrict 6])
696 {
697 for (int i = 0; i < 6; i++) {
698 for (int j = 0; j < 6; j++)
699 mv[i] += m[i][j] * v[j];
700 }
701 }
702
703 /*----------------------------------------------------------------------------*/
704 /*!
705 * \brief Compute the determinant of a 3x3 matrix
706 *
707 * \param[in] m 3x3 matrix
708 *
709 * \return the determinant
710 */
711 /*----------------------------------------------------------------------------*/
712
713 static inline cs_real_t
cs_math_33_determinant(const cs_real_t m[3][3])714 cs_math_33_determinant(const cs_real_t m[3][3])
715 {
716 const cs_real_t com0 = m[1][1]*m[2][2] - m[2][1]*m[1][2];
717 const cs_real_t com1 = m[2][1]*m[0][2] - m[0][1]*m[2][2];
718 const cs_real_t com2 = m[0][1]*m[1][2] - m[1][1]*m[0][2];
719
720 return m[0][0]*com0 + m[1][0]*com1 + m[2][0]*com2;
721 }
722
723 /*----------------------------------------------------------------------------*/
724 /*!
725 * \brief Compute the determinant of a 3x3 symmetric matrix
726 *
727 * \param[in] m 3x3 symmetric matrix
728 *
729 * \return the determinant
730 */
731 /*----------------------------------------------------------------------------*/
732
733 static inline cs_real_t
cs_math_sym_33_determinant(const cs_real_6_t m)734 cs_math_sym_33_determinant(const cs_real_6_t m)
735 {
736 const cs_real_t com0 = m[1]*m[2] - m[4]*m[4];
737 const cs_real_t com1 = m[4]*m[5] - m[3]*m[2];
738 const cs_real_t com2 = m[3]*m[4] - m[1]*m[5];
739
740 return m[0]*com0 + m[3]*com1 + m[5]*com2;
741 }
742
743 /*----------------------------------------------------------------------------*/
744 /*!
745 * \brief Compute the cross product of two vectors of 3 real values.
746 *
747 * \param[in] u vector of 3 real values
748 * \param[in] v vector of 3 real values
749 * \param[out] uv cross-product of u an v
750 */
751 /*----------------------------------------------------------------------------*/
752
753 #if defined(__INTEL_COMPILER)
754 #pragma optimization_level 0 /* Bug with O1 or above with icc 15.0.1 20141023 */
755 #endif
756
757 static inline void
cs_math_3_cross_product(const cs_real_t u[3],const cs_real_t v[3],cs_real_t uv[restrict3])758 cs_math_3_cross_product(const cs_real_t u[3],
759 const cs_real_t v[3],
760 cs_real_t uv[restrict 3])
761 {
762 uv[0] = u[1]*v[2] - u[2]*v[1];
763 uv[1] = u[2]*v[0] - u[0]*v[2];
764 uv[2] = u[0]*v[1] - u[1]*v[0];
765 }
766
767 /*----------------------------------------------------------------------------*/
768 /*!
769 * \brief Compute the triple product
770 *
771 * \param[in] u vector of 3 real values
772 * \param[in] v vector of 3 real values
773 * \param[in] w vector of 3 real values
774 *
775 * \return the scalar triple product: (uxv).w
776 */
777 /*----------------------------------------------------------------------------*/
778
779 #if defined(__INTEL_COMPILER)
780 #pragma optimization_level 0 /* Bug with O1 or above with icc 15.0.1 20141023 */
781 #endif
782
783 static inline cs_real_t
cs_math_3_triple_product(const cs_real_t u[3],const cs_real_t v[3],const cs_real_t w[3])784 cs_math_3_triple_product(const cs_real_t u[3],
785 const cs_real_t v[3],
786 const cs_real_t w[3])
787 {
788 return (u[1]*v[2] - u[2]*v[1]) * w[0]
789 + (u[2]*v[0] - u[0]*v[2]) * w[1]
790 + (u[0]*v[1] - u[1]*v[0]) * w[2];
791 }
792
793 /*----------------------------------------------------------------------------*/
794 /*!
795 * \brief Inverse a 3x3 matrix
796 *
797 * \param[in] in matrix to inverse
798 * \param[out] out inversed matrix
799 */
800 /*----------------------------------------------------------------------------*/
801
802 static inline void
cs_math_33_inv_cramer(const cs_real_t in[3][3],cs_real_t out[3][3])803 cs_math_33_inv_cramer(const cs_real_t in[3][3],
804 cs_real_t out[3][3])
805 {
806 out[0][0] = in[1][1]*in[2][2] - in[2][1]*in[1][2];
807 out[0][1] = in[2][1]*in[0][2] - in[0][1]*in[2][2];
808 out[0][2] = in[0][1]*in[1][2] - in[1][1]*in[0][2];
809
810 out[1][0] = in[2][0]*in[1][2] - in[1][0]*in[2][2];
811 out[1][1] = in[0][0]*in[2][2] - in[2][0]*in[0][2];
812 out[1][2] = in[1][0]*in[0][2] - in[0][0]*in[1][2];
813
814 out[2][0] = in[1][0]*in[2][1] - in[2][0]*in[1][1];
815 out[2][1] = in[2][0]*in[0][1] - in[0][0]*in[2][1];
816 out[2][2] = in[0][0]*in[1][1] - in[1][0]*in[0][1];
817
818 const double det = in[0][0]*out[0][0]+in[1][0]*out[0][1]+in[2][0]*out[0][2];
819 const double invdet = 1/det;
820
821 out[0][0] *= invdet, out[0][1] *= invdet, out[0][2] *= invdet;
822 out[1][0] *= invdet, out[1][1] *= invdet, out[1][2] *= invdet;
823 out[2][0] *= invdet, out[2][1] *= invdet, out[2][2] *= invdet;
824 }
825
826 /*----------------------------------------------------------------------------*/
827 /*!
828 * \brief Inverse a 3x3 matrix in place, using Cramer's rule
829 *
830 * \param[in, out] a matrix to inverse
831 */
832 /*----------------------------------------------------------------------------*/
833
834 static inline void
cs_math_33_inv_cramer_in_place(cs_real_t a[3][3])835 cs_math_33_inv_cramer_in_place(cs_real_t a[3][3])
836 {
837 cs_real_t a00 = a[1][1]*a[2][2] - a[2][1]*a[1][2];
838 cs_real_t a01 = a[2][1]*a[0][2] - a[0][1]*a[2][2];
839 cs_real_t a02 = a[0][1]*a[1][2] - a[1][1]*a[0][2];
840 cs_real_t a10 = a[2][0]*a[1][2] - a[1][0]*a[2][2];
841 cs_real_t a11 = a[0][0]*a[2][2] - a[2][0]*a[0][2];
842 cs_real_t a12 = a[1][0]*a[0][2] - a[0][0]*a[1][2];
843 cs_real_t a20 = a[1][0]*a[2][1] - a[2][0]*a[1][1];
844 cs_real_t a21 = a[2][0]*a[0][1] - a[0][0]*a[2][1];
845 cs_real_t a22 = a[0][0]*a[1][1] - a[1][0]*a[0][1];
846
847 double det_inv = 1. / (a[0][0]*a00 + a[1][0]*a01 + a[2][0]*a02);
848
849 a[0][0] = a00 * det_inv;
850 a[0][1] = a01 * det_inv;
851 a[0][2] = a02 * det_inv;
852 a[1][0] = a10 * det_inv;
853 a[1][1] = a11 * det_inv;
854 a[1][2] = a12 * det_inv;
855 a[2][0] = a20 * det_inv;
856 a[2][1] = a21 * det_inv;
857 a[2][2] = a22 * det_inv;
858 }
859
860 /*----------------------------------------------------------------------------*/
861 /*!
862 * \brief Inverse a 3x3 symmetric matrix (with non-symmetric storage)
863 * in place, using Cramer's rule
864 *
865 * \param[in, out] a matrix to inverse
866 */
867 /*----------------------------------------------------------------------------*/
868
869 static inline void
cs_math_33_inv_cramer_sym_in_place(cs_real_t a[3][3])870 cs_math_33_inv_cramer_sym_in_place(cs_real_t a[3][3])
871 {
872 cs_real_t a00 = a[1][1]*a[2][2] - a[2][1]*a[1][2];
873 cs_real_t a01 = a[2][1]*a[0][2] - a[0][1]*a[2][2];
874 cs_real_t a02 = a[0][1]*a[1][2] - a[1][1]*a[0][2];
875 cs_real_t a11 = a[0][0]*a[2][2] - a[2][0]*a[0][2];
876 cs_real_t a12 = a[1][0]*a[0][2] - a[0][0]*a[1][2];
877 cs_real_t a22 = a[0][0]*a[1][1] - a[1][0]*a[0][1];
878
879 double det_inv = 1. / (a[0][0]*a00 + a[1][0]*a01 + a[2][0]*a02);
880
881 a[0][0] = a00 * det_inv;
882 a[0][1] = a01 * det_inv;
883 a[0][2] = a02 * det_inv;
884 a[1][0] = a01 * det_inv;
885 a[1][1] = a11 * det_inv;
886 a[1][2] = a12 * det_inv;
887 a[2][0] = a02 * det_inv;
888 a[2][1] = a12 * det_inv;
889 a[2][2] = a22 * det_inv;
890 }
891
892 /*----------------------------------------------------------------------------*/
893 /*!
894 * \brief Compute the inverse of a symmetric matrix using Cramer's rule.
895 *
896 * \remark Symmetric matrix coefficients are stored as follows:
897 * (s11, s22, s33, s12, s23, s13)
898 *
899 * \param[in] s symmetric matrix
900 * \param[out] sout sout = 1/s1
901 */
902 /*----------------------------------------------------------------------------*/
903
904 static inline void
cs_math_sym_33_inv_cramer(const cs_real_t s[6],cs_real_t sout[restrict6])905 cs_math_sym_33_inv_cramer(const cs_real_t s[6],
906 cs_real_t sout[restrict 6])
907 {
908 double detinv;
909
910 sout[0] = s[1]*s[2] - s[4]*s[4];
911 sout[1] = s[0]*s[2] - s[5]*s[5];
912 sout[2] = s[0]*s[1] - s[3]*s[3];
913 sout[3] = s[4]*s[5] - s[3]*s[2];
914 sout[4] = s[3]*s[5] - s[0]*s[4];
915 sout[5] = s[3]*s[4] - s[1]*s[5];
916
917 detinv = 1. / (s[0]*sout[0] + s[3]*sout[3] + s[5]*sout[5]);
918
919 sout[0] *= detinv;
920 sout[1] *= detinv;
921 sout[2] *= detinv;
922 sout[3] *= detinv;
923 sout[4] *= detinv;
924 sout[5] *= detinv;
925 }
926
927 /*----------------------------------------------------------------------------*/
928 /*!
929 * \brief Compute the product of two 3x3 real valued matrices.
930 *
931 * \param[in] m1 matrix of 3x3 real values
932 * \param[in] m2 matrix of 3x3 real values
933 * \param[out] mout m1.m2 product
934 */
935 /*----------------------------------------------------------------------------*/
936
937 static inline void
cs_math_33_product(const cs_real_t m1[3][3],const cs_real_t m2[3][3],cs_real_t mout[3][3])938 cs_math_33_product(const cs_real_t m1[3][3],
939 const cs_real_t m2[3][3],
940 cs_real_t mout[3][3])
941 {
942 mout[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
943 mout[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
944 mout[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
945
946 mout[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
947 mout[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
948 mout[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
949
950 mout[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
951 mout[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
952 mout[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
953 }
954
955 /*----------------------------------------------------------------------------*/
956 /*!
957 * \brief Compute transformation from relative to absolute
958 * reference frame Q^t M Q
959 *
960 * \param[in] m matrix of 3x3 real values
961 * \param[in] q transformation matrix of 3x3 real values
962 * \param[out] mout Q^t M Q
963 */
964 /*----------------------------------------------------------------------------*/
965
966 static inline void
cs_math_33_transform_r_to_a(const cs_real_t m[3][3],const cs_real_t q[3][3],cs_real_t mout[3][3])967 cs_math_33_transform_r_to_a(const cs_real_t m[3][3],
968 const cs_real_t q[3][3],
969 cs_real_t mout[3][3])
970 {
971 /* _m = M.Q */
972 cs_real_33_t _m;
973 _m[0][0] = m[0][0]*q[0][0] + m[0][1]*q[1][0] + m[0][2]*q[2][0];
974 _m[0][1] = m[0][0]*q[0][1] + m[0][1]*q[1][1] + m[0][2]*q[2][1];
975 _m[0][2] = m[0][0]*q[0][2] + m[0][1]*q[1][2] + m[0][2]*q[2][2];
976
977 _m[1][0] = m[1][0]*q[0][0] + m[1][1]*q[1][0] + m[1][2]*q[2][0];
978 _m[1][1] = m[1][0]*q[0][1] + m[1][1]*q[1][1] + m[1][2]*q[2][1];
979 _m[1][2] = m[1][0]*q[0][2] + m[1][1]*q[1][2] + m[1][2]*q[2][2];
980
981 _m[2][0] = m[2][0]*q[0][0] + m[2][1]*q[1][0] + m[2][2]*q[2][0];
982 _m[2][1] = m[2][0]*q[0][1] + m[2][1]*q[1][1] + m[2][2]*q[2][1];
983 _m[2][2] = m[2][0]*q[0][2] + m[2][1]*q[1][2] + m[2][2]*q[2][2];
984
985 /* mout = Q^t _m */
986 mout[0][0] = q[0][0]*_m[0][0] + q[1][0]*_m[1][0] + q[2][0]*_m[2][0];
987 mout[0][1] = q[0][0]*_m[0][1] + q[1][0]*_m[1][1] + q[2][0]*_m[2][1];
988 mout[0][2] = q[0][0]*_m[0][2] + q[1][0]*_m[1][2] + q[2][0]*_m[2][2];
989
990 mout[1][0] = q[0][1]*_m[0][0] + q[1][1]*_m[1][0] + q[2][1]*_m[2][0];
991 mout[1][1] = q[0][1]*_m[0][1] + q[1][1]*_m[1][1] + q[2][1]*_m[2][1];
992 mout[1][2] = q[0][1]*_m[0][2] + q[1][1]*_m[1][2] + q[2][1]*_m[2][2];
993
994 mout[2][0] = q[0][2]*_m[0][0] + q[1][2]*_m[1][0] + q[2][2]*_m[2][0];
995 mout[2][1] = q[0][2]*_m[0][1] + q[1][2]*_m[1][1] + q[2][2]*_m[2][1];
996 mout[2][2] = q[0][2]*_m[0][2] + q[1][2]*_m[1][2] + q[2][2]*_m[2][2];
997 }
998
999 /*----------------------------------------------------------------------------*/
1000 /*!
1001 * \brief Compute transformation from absolute to relative
1002 * reference frame Q M Q^t
1003 *
1004 * \param[in] m matrix of 3x3 real values
1005 * \param[in] q transformation matrix of 3x3 real values
1006 * \param[out] mout Q M Q^t
1007 */
1008 /*----------------------------------------------------------------------------*/
1009
1010 static inline void
cs_math_33_transform_a_to_r(const cs_real_t m[3][3],const cs_real_t q[3][3],cs_real_t mout[3][3])1011 cs_math_33_transform_a_to_r(const cs_real_t m[3][3],
1012 const cs_real_t q[3][3],
1013 cs_real_t mout[3][3])
1014 {
1015 /* _m = M.Q^t */
1016 cs_real_33_t _m;
1017 _m[0][0] = m[0][0]*q[0][0] + m[0][1]*q[0][1] + m[0][2]*q[0][2];
1018 _m[0][1] = m[0][0]*q[1][0] + m[0][1]*q[1][1] + m[0][2]*q[1][2];
1019 _m[0][2] = m[0][0]*q[2][0] + m[0][1]*q[2][1] + m[0][2]*q[2][2];
1020
1021 _m[1][0] = m[1][0]*q[0][0] + m[1][1]*q[0][1] + m[1][2]*q[0][2];
1022 _m[1][1] = m[1][0]*q[1][0] + m[1][1]*q[1][1] + m[1][2]*q[1][2];
1023 _m[1][2] = m[1][0]*q[2][0] + m[1][1]*q[2][1] + m[1][2]*q[2][2];
1024
1025 _m[2][0] = m[2][0]*q[0][0] + m[2][1]*q[0][1] + m[2][2]*q[0][2];
1026 _m[2][1] = m[2][0]*q[1][0] + m[2][1]*q[1][1] + m[2][2]*q[1][2];
1027 _m[2][2] = m[2][0]*q[2][0] + m[2][1]*q[2][1] + m[2][2]*q[2][2];
1028
1029 /* mout = Q _m */
1030 mout[0][0] = q[0][0]*_m[0][0] + q[0][1]*_m[1][0] + q[0][2]*_m[2][0];
1031 mout[0][1] = q[0][0]*_m[0][1] + q[0][1]*_m[1][1] + q[0][2]*_m[2][1];
1032 mout[0][2] = q[0][0]*_m[0][2] + q[0][1]*_m[1][2] + q[0][2]*_m[2][2];
1033
1034 mout[1][0] = q[1][0]*_m[0][0] + q[1][1]*_m[1][0] + q[1][2]*_m[2][0];
1035 mout[1][1] = q[1][0]*_m[0][1] + q[1][1]*_m[1][1] + q[1][2]*_m[2][1];
1036 mout[1][2] = q[1][0]*_m[0][2] + q[1][1]*_m[1][2] + q[1][2]*_m[2][2];
1037
1038 mout[2][0] = q[2][0]*_m[0][0] + q[2][1]*_m[1][0] + q[2][2]*_m[2][0];
1039 mout[2][1] = q[2][0]*_m[0][1] + q[2][1]*_m[1][1] + q[2][2]*_m[2][1];
1040 mout[2][2] = q[2][0]*_m[0][2] + q[2][1]*_m[1][2] + q[2][2]*_m[2][2];
1041 }
1042
1043 /*----------------------------------------------------------------------------*/
1044 /*!
1045 * \brief Extract from the given matrix its symmetric
1046 * and anti-symmetric part
1047 *
1048 * \param[in] m matrix of 3x3 real values
1049 * \param[out] m_sym matrix of 3x3 real values (symmetric part)
1050 * \param[out] m_ant matrix of 3x3 real values (anti-symmetric part)
1051 */
1052 /*----------------------------------------------------------------------------*/
1053
1054 static inline void
cs_math_33_extract_sym_ant(const cs_real_t m[3][3],cs_real_t m_sym[3][3],cs_real_t m_ant[3][3])1055 cs_math_33_extract_sym_ant(const cs_real_t m[3][3],
1056 cs_real_t m_sym[3][3],
1057 cs_real_t m_ant[3][3])
1058 {
1059 /* sym = 0.5 (m + m_transpose) */
1060 m_sym[0][0] = 0.5 * (m[0][0] + m[0][0]);
1061 m_sym[0][1] = 0.5 * (m[0][1] + m[1][0]);
1062 m_sym[0][2] = 0.5 * (m[0][2] + m[2][0]);
1063 m_sym[1][0] = 0.5 * (m[1][0] + m[0][1]);
1064 m_sym[1][1] = 0.5 * (m[1][1] + m[1][1]);
1065 m_sym[1][2] = 0.5 * (m[1][2] + m[2][1]);
1066 m_sym[2][0] = 0.5 * (m[2][0] + m[0][2]);
1067 m_sym[2][1] = 0.5 * (m[2][1] + m[1][2]);
1068 m_sym[2][2] = 0.5 * (m[2][2] + m[2][2]);
1069
1070 /* ant = 0.5 (m - m_transpose) */
1071 m_ant[0][0] = 0.5 * (m[0][0] - m[0][0]);
1072 m_ant[0][1] = 0.5 * (m[0][1] - m[1][0]);
1073 m_ant[0][2] = 0.5 * (m[0][2] - m[2][0]);
1074 m_ant[1][0] = 0.5 * (m[1][0] - m[0][1]);
1075 m_ant[1][1] = 0.5 * (m[1][1] - m[1][1]);
1076 m_ant[1][2] = 0.5 * (m[1][2] - m[2][1]);
1077 m_ant[2][0] = 0.5 * (m[2][0] - m[0][2]);
1078 m_ant[2][1] = 0.5 * (m[2][1] - m[1][2]);
1079 m_ant[2][2] = 0.5 * (m[2][2] - m[2][2]);
1080 }
1081
1082 /*----------------------------------------------------------------------------*/
1083 /*!
1084 * \brief Add the product of two 3x3 real matrices to a matrix.
1085 *
1086 * \param[in] m1 matrix of 3x3 real values
1087 * \param[in] m2 matrix of 3x3 real values
1088 * \param[out] mout matrix of 3x3 real values
1089 */
1090 /*----------------------------------------------------------------------------*/
1091
1092 static inline void
cs_math_33_product_add(const cs_real_t m1[3][3],const cs_real_t m2[3][3],cs_real_t mout[restrict3][3])1093 cs_math_33_product_add(const cs_real_t m1[3][3],
1094 const cs_real_t m2[3][3],
1095 cs_real_t mout[restrict 3][3])
1096 {
1097 mout[0][0] += m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
1098 mout[0][1] += m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
1099 mout[0][2] += m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
1100
1101 mout[1][0] += m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
1102 mout[1][1] += m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
1103 mout[1][2] += m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
1104
1105 mout[2][0] += m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
1106 mout[2][1] += m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
1107 mout[2][2] += m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
1108 }
1109
1110 /*----------------------------------------------------------------------------*/
1111 /*!
1112 * \brief Compute the product of two symmetric matrices.
1113 *
1114 * Warning: this is valid if and only if s1 and s2 commute (otherwise sout is
1115 * not symmetric).
1116 *
1117 * \remark Symmetric matrix coefficients are stored as follows:
1118 * (s11, s22, s33, s12, s23, s13)
1119 *
1120 * \param[in] s1 symmetric matrix
1121 * \param[in] s2 symmetric matrix
1122 * \param[out] sout sout = s1 * s2
1123 */
1124 /*----------------------------------------------------------------------------*/
1125
1126 static inline void
cs_math_sym_33_product(const cs_real_t s1[6],const cs_real_t s2[6],cs_real_t sout[restrict6])1127 cs_math_sym_33_product(const cs_real_t s1[6],
1128 const cs_real_t s2[6],
1129 cs_real_t sout[restrict 6])
1130 {
1131 /* S11 */
1132 sout[0] = s1[0]*s2[0] + s1[3]*s2[3] + s1[5]*s2[5];
1133 /* S22 */
1134 sout[1] = s1[3]*s2[3] + s1[1]*s2[1] + s1[4]*s2[4];
1135 /* S33 */
1136 sout[2] = s1[5]*s2[5] + s1[4]*s2[4] + s1[2]*s2[2];
1137 /* S12 = S21 */
1138 sout[3] = s1[0]*s2[3] + s1[3]*s2[1] + s1[5]*s2[4];
1139 /* S23 = S32 */
1140 sout[4] = s1[3]*s2[5] + s1[1]*s2[4] + s1[4]*s2[2];
1141 /* S13 = S31 */
1142 sout[5] = s1[0]*s2[5] + s1[3]*s2[4] + s1[5]*s2[2];
1143 }
1144
1145 /*----------------------------------------------------------------------------*/
1146 /*!
1147 * \brief Compute a 6x6 matrix A, equivalent to a 3x3 matrix s, such as:
1148 * A*R_6 = R*s^t + s*R
1149 *
1150 * \param[in] s 3x3 matrix
1151 * \param[out] sout 6x6 matrix
1152 */
1153 /*----------------------------------------------------------------------------*/
1154
1155 static inline void
cs_math_reduce_sym_prod_33_to_66(const cs_real_t s[3][3],cs_real_t sout[restrict6][6])1156 cs_math_reduce_sym_prod_33_to_66(const cs_real_t s[3][3],
1157 cs_real_t sout[restrict 6][6])
1158 {
1159 int tens2vect[3][3];
1160 int iindex[6], jindex[6];
1161
1162 tens2vect[0][0] = 0; tens2vect[0][1] = 3; tens2vect[0][2] = 5;
1163 tens2vect[1][0] = 3; tens2vect[1][1] = 1; tens2vect[1][2] = 4;
1164 tens2vect[2][0] = 5; tens2vect[2][1] = 4; tens2vect[2][2] = 2;
1165
1166 iindex[0] = 0; iindex[1] = 1; iindex[2] = 2;
1167 iindex[3] = 0; iindex[4] = 1; iindex[5] = 0;
1168
1169 jindex[0] = 0; jindex[1] = 1; jindex[2] = 2;
1170 jindex[3] = 1; jindex[4] = 2; jindex[5] = 2;
1171
1172 /* Consider : W = R*s^t + s*R.
1173 * W_ij = Sum_{k<3} [s_jk*r_ik + s_ik*r_jk]
1174 * We look for A such as A*R = W
1175 */
1176 for (int i = 0; i < 6; i++) {
1177 int ii = iindex[i];
1178 int jj = jindex[i];
1179 for (int k = 0; k < 3; k++) {
1180 int ik = tens2vect[k][ii];
1181 int jk = tens2vect[k][jj];
1182
1183 sout[ik][i] += s[k][jj];
1184
1185 sout[jk][i] += s[k][ii];
1186 }
1187 }
1188 }
1189
1190 /*----------------------------------------------------------------------------*/
1191 /*!
1192 * \brief Compute the product of three symmetric matrices.
1193 *
1194 * \remark Symmetric matrix coefficients are stored as follows:
1195 * (s11, s22, s33, s12, s23, s13)
1196 *
1197 * \param[in] s1 symmetric matrix
1198 * \param[in] s2 symmetric matrix
1199 * \param[in] s3 symmetric matrix
1200 * \param[out] sout sout = s1 * s2 * s3
1201 */
1202 /*----------------------------------------------------------------------------*/
1203
1204 static inline void
cs_math_sym_33_double_product(const cs_real_t s1[6],const cs_real_t s2[6],const cs_real_t s3[6],cs_real_t sout[restrict3][3])1205 cs_math_sym_33_double_product(const cs_real_t s1[6],
1206 const cs_real_t s2[6],
1207 const cs_real_t s3[6],
1208 cs_real_t sout[restrict 3][3])
1209 {
1210 cs_real_t _sout[3][3];
1211
1212 /* S11 */
1213 _sout[0][0] = s1[0]*s2[0] + s1[3]*s2[3] + s1[5]*s2[5];
1214 /* S22 */
1215 _sout[1][1] = s1[3]*s2[3] + s1[1]*s2[1] + s1[4]*s2[4];
1216 /* S33 */
1217 _sout[2][2] = s1[5]*s2[5] + s1[4]*s2[4] + s1[2]*s2[2];
1218 /* S12 */
1219 _sout[0][1] = s1[0]*s2[3] + s1[3]*s2[1] + s1[5]*s2[4];
1220 /* S21 */
1221 _sout[1][0] = s2[0]*s1[3] + s2[3]*s1[1] + s2[5]*s1[4];
1222 /* S23 */
1223 _sout[1][2] = s1[3]*s2[5] + s1[1]*s2[4] + s1[4]*s2[2];
1224 /* S32 */
1225 _sout[2][1] = s2[3]*s1[5] + s2[1]*s1[4] + s2[4]*s1[2];
1226 /* S13 */
1227 _sout[0][2] = s1[0]*s2[5] + s1[3]*s2[4] + s1[5]*s2[2];
1228 /* S31 */
1229 _sout[2][0] = s2[0]*s1[5] + s2[3]*s1[4] + s2[5]*s1[2];
1230
1231 sout[0][0] = _sout[0][0]*s3[0] + _sout[0][1]*s3[3] + _sout[0][2]*s3[5];
1232 /* S22 */
1233 sout[1][1] = _sout[1][0]*s3[3] + _sout[1][1]*s3[1] + _sout[1][2]*s3[4];
1234 /* S33 */
1235 sout[2][2] = _sout[2][0]*s3[5] + _sout[2][1]*s3[4] + _sout[2][2]*s3[2];
1236 /* S12 */
1237 sout[0][1] = _sout[0][0]*s3[3] + _sout[0][1]*s3[1] + _sout[0][2]*s3[4];
1238 /* S21 */
1239 sout[1][0] = s3[0]*_sout[1][0] + s3[3]*_sout[1][1] + s3[5]*_sout[1][2];
1240 /* S23 */
1241 sout[1][2] = _sout[1][0]*s3[5] + _sout[1][1]*s3[4] + _sout[1][2]*s3[2];
1242 /* S32 */
1243 sout[2][1] = s3[3]*_sout[2][0] + s3[1]*_sout[2][1] + s3[4]*_sout[2][2];
1244 /* S13 */
1245 sout[0][2] = _sout[0][0]*s3[5] + _sout[0][1]*s3[4] + _sout[0][2]*s3[2];
1246 /* S31 */
1247 sout[2][0] = s3[0]*_sout[2][0] + s3[3]*_sout[2][1] + s3[5]*_sout[2][2];
1248 }
1249
1250 /*----------------------------------------------------------------------------*/
1251 /*!
1252 * \brief Define a cs_nvec3_t structure from a cs_real_3_t
1253 *
1254 * \param[in] v vector of size 3
1255 * \param[out] qv pointer to a cs_nvec3_t structure
1256 */
1257 /*----------------------------------------------------------------------------*/
1258
1259 static inline void
cs_nvec3(const cs_real_3_t v,cs_nvec3_t * qv)1260 cs_nvec3(const cs_real_3_t v,
1261 cs_nvec3_t *qv)
1262 {
1263 cs_real_t magnitude = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
1264
1265 qv->meas = magnitude;
1266 if (fabs(magnitude) > cs_math_zero_threshold) {
1267
1268 const cs_real_t inv = 1/magnitude;
1269 qv->unitv[0] = inv * v[0];
1270 qv->unitv[1] = inv * v[1];
1271 qv->unitv[2] = inv * v[2];
1272
1273 }
1274 else
1275 qv->unitv[0] = qv->unitv[1] = qv->unitv[2] = 0;
1276 }
1277
1278 /*=============================================================================
1279 * Public function prototypes
1280 *============================================================================*/
1281
1282 /*----------------------------------------------------------------------------*/
1283 /*!
1284 * \brief Compute the value related to the machine precision
1285 */
1286 /*----------------------------------------------------------------------------*/
1287
1288 void
1289 cs_math_set_machine_epsilon(void);
1290
1291 /*----------------------------------------------------------------------------*/
1292 /*!
1293 * \brief Get the value related to the machine precision
1294 */
1295 /*----------------------------------------------------------------------------*/
1296
1297 double
1298 cs_math_get_machine_epsilon(void);
1299
1300 /*----------------------------------------------------------------------------*/
1301 /*!
1302 * \brief Compute the length (Euclidean norm) between two points xa and xb in
1303 * a Cartesian coordinate system of dimension 3
1304 *
1305 * \param[in] xa coordinate of the first extremity
1306 * \param[in] xb coordinate of the second extremity
1307 * \param[out] len pointer to the length of the vector va -> vb
1308 * \param[out] unitv unitary vector along xa -> xb
1309 */
1310 /*----------------------------------------------------------------------------*/
1311
1312 void
1313 cs_math_3_length_unitv(const cs_real_t xa[3],
1314 const cs_real_t xb[3],
1315 cs_real_t *len,
1316 cs_real_3_t unitv);
1317
1318 /*----------------------------------------------------------------------------*/
1319 /*!
1320 * \brief Compute all eigenvalues of a 3x3 symmetric matrix
1321 * with symmetric storage.
1322 *
1323 * Based on: Oliver K. Smith "eigenvalues of a symmetric 3x3 matrix",
1324 * Communication of the ACM (April 1961)
1325 * (Wikipedia article entitled "Eigenvalue algorithm")
1326 *
1327 * \param[in] m 3x3 symmetric matrix (m11, m22, m33, m12, m23, m13)
1328 * \param[out] eig_vals size 3 vector
1329 */
1330 /*----------------------------------------------------------------------------*/
1331
1332 void
1333 cs_math_sym_33_eigen(const cs_real_t m[6],
1334 cs_real_t eig_vals[3]);
1335
1336 /*----------------------------------------------------------------------------*/
1337 /*!
1338 * \brief Compute max/min eigenvalues ratio and max. eigenvalue of a 3x3
1339 * symmetric matrix with non-symmetric storage.
1340 *
1341 * Based on: Oliver K. Smith "eigenvalues of a symmetric 3x3 matrix",
1342 * Communication of the ACM (April 1961)
1343 * (Wikipedia article entitled "Eigenvalue algorithm")
1344 *
1345 * \param[in] m 3x3 matrix
1346 * \param[out] eig_ratio max/min
1347 * \param[out] eig_max max. eigenvalue
1348 */
1349 /*----------------------------------------------------------------------------*/
1350
1351 void
1352 cs_math_33_eigen(const cs_real_t m[3][3],
1353 cs_real_t *eig_ratio,
1354 cs_real_t *eig_max);
1355
1356 /*----------------------------------------------------------------------------*/
1357 /*!
1358 * \brief Compute the area of the convex_hull generated by 3 points.
1359 * This corresponds to the computation of the surface of a triangle
1360 *
1361 * \param[in] xv coordinates of the first vertex
1362 * \param[in] xe coordinates of the second vertex
1363 * \param[in] xf coordinates of the third vertex
1364 *
1365 * \return the surface of a triangle
1366 */
1367 /*----------------------------------------------------------------------------*/
1368
1369 double
1370 cs_math_surftri(const cs_real_t xv[3],
1371 const cs_real_t xe[3],
1372 const cs_real_t xf[3]);
1373
1374 /*----------------------------------------------------------------------------*/
1375 /*!
1376 * \brief Compute the volume of the convex_hull generated by 4 points.
1377 * This is equivalent to the computation of the volume of a tetrahedron
1378 *
1379 * \param[in] xv coordinates of the first vertex
1380 * \param[in] xe coordinates of the second vertex
1381 * \param[in] xf coordinates of the third vertex
1382 * \param[in] xc coordinates of the fourth vertex
1383 *
1384 * \return the volume of the tetrahedron.
1385 */
1386 /*----------------------------------------------------------------------------*/
1387
1388 double
1389 cs_math_voltet(const cs_real_t xv[3],
1390 const cs_real_t xe[3],
1391 const cs_real_t xf[3],
1392 const cs_real_t xc[3]);
1393
1394 /*----------------------------------------------------------------------------*/
1395 /*!
1396 * \brief Evaluate eigenvalues and eigenvectors
1397 * of a real symmetric matrix m1[3,3]: m1*m2 = lambda*m2
1398 *
1399 * Use of Jacobi method for symmetric matrices
1400 * (adapted from the book Numerical Recipes in C, Chapter 11.1)
1401 *
1402 * \param[in] m_in matrix of 3x3 real values (initial)
1403 * \param[in] tol_err absolute tolerance (sum of off-diagonal elements)
1404 * \param[out] eig_val vector of 3 real values (eigenvalues)
1405 * \param[out] eig_vec matrix of 3x3 real values (eigenvectors)
1406 */
1407 /*----------------------------------------------------------------------------*/
1408
1409 void
1410 cs_math_33_eig_val_vec(const cs_real_t m_in[3][3],
1411 const cs_real_t tol_err,
1412 cs_real_t eig_val[restrict 3],
1413 cs_real_t eig_vec[restrict 3][3]);
1414
1415 /*----------------------------------------------------------------------------*/
1416 /*!
1417 * \brief Compute LU factorization of an array of dense matrices
1418 * of identical size.
1419 *
1420 * \param[in] n_blocks number of blocks
1421 * \param[in] b_size block size
1422 * \param[in] a matrix blocks
1423 * \param[out] a_lu LU factorizations of matrix blocks
1424 */
1425 /*----------------------------------------------------------------------------*/
1426
1427 void
1428 cs_math_fact_lu(cs_lnum_t n_blocks,
1429 const int b_size,
1430 const cs_real_t *a,
1431 cs_real_t *a_lu);
1432
1433 /*----------------------------------------------------------------------------*/
1434 /*!
1435 * \brief Block Jacobi utilities.
1436 * Compute forward and backward to solve an LU P*P system.
1437 *
1438 * \param[in] a_lu matrix LU factorization
1439 * \param[in] n matrix size
1440 * \param[out] x solution
1441 * \param[out] b right hand side
1442 */
1443 /*----------------------------------------------------------------------------*/
1444
1445 void
1446 cs_math_fw_and_bw_lu(const cs_real_t a_lu[],
1447 const int n,
1448 cs_real_t x[],
1449 const cs_real_t b[]);
1450
1451 /*----------------------------------------------------------------------------*/
1452
1453 END_C_DECLS
1454
1455 #endif /* __CS_MATH_H__ */
1456