cs_math.h

Go to the documentation of this file.

#ifndef CS_MATH_H
#define CS_MATH_H
 
/*============================================================================
 * Mathematical base functions.
 *============================================================================*/
 
/*
  This file is part of code_saturne, a general-purpose CFD tool.
 
  Copyright (C) 1998-2026 EDF S.A.
 
  This program is free software; you can redistribute it and/or modify it under
  the terms of the GNU General Public License as published by the Free Software
  Foundation; either version 2 of the License, or (at your option) any later
  version.
 
  This program is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  details.
 
  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
  Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
 
/*----------------------------------------------------------------------------*/
 
#include "base/cs_defs.h"
 
/*----------------------------------------------------------------------------
 * Standard C library headers
 *----------------------------------------------------------------------------*/
 
#include <assert.h>
#include <math.h>
 
#if  (defined(__NVCC__) && defined(__CUDA_ARCH__)) \
  || (defined(__HIPCC__) && defined(__HIP_DEVICE_COMPILE__)) \
  || defined(SYCL_LANGUAGE_VERSION) \
  || defined(HAVE_OPENMP_TARGET)
#include <float.h>
#endif
 
#if defined(__cplusplus)
#include <limits>
#endif
 
/*----------------------------------------------------------------------------
 *  Local headers
 *----------------------------------------------------------------------------*/
 
#if defined(DEBUG) && !defined(NDEBUG) /* Sanity check */
#include "bft/bft_error.h"
#endif
 
/*----------------------------------------------------------------------------*/
 
BEGIN_C_DECLS
 
/*=============================================================================
 * Local Macro definitions
 *============================================================================*/
 
/*============================================================================
 * Type definition
 *============================================================================*/
 
/* Symmetric tensor component name */
 
typedef enum {
 
  XX,
  YY,
  ZZ,
  XY,
  YZ,
  XZ
 
} cs_math_sym_tensor_component_t;
 
/*============================================================================
 *  Global variables
 *============================================================================*/
 
/* Numerical constants */
 
static constexpr cs_real_t cs_math_zero_threshold = std::numeric_limits<float>::min();
 
static constexpr double cs_dbl_epsilon = std::numeric_limits<double>::epsilon();
 
static constexpr double cs_dbl_min = std::numeric_limits<double>::min();
 
static constexpr double cs_dbl_max = std::numeric_limits<double>::max();
 
static constexpr cs_real_t cs_math_epzero = 1e-12;
 
static constexpr cs_real_t cs_math_infinite_r = 1.e30;
 
static constexpr cs_real_t cs_math_big_r = 1.e12;
 
static constexpr cs_real_t cs_math_pi = 3.14159265358979323846;
 
static constexpr cs_real_t cs_math_1ov3  = 1./3.;
static constexpr cs_real_t cs_math_2ov3  = 2./3.;
static constexpr cs_real_t cs_math_4ov3  = 4./3.;
static constexpr cs_real_t cs_math_5ov3  = 5./3.;
static constexpr cs_real_t cs_math_1ov6  = 1./6.;
static constexpr cs_real_t cs_math_1ov12 = 1./12.;
static constexpr cs_real_t cs_math_1ov24 = 1./24.;
 
static constexpr cs_real_33_t cs_math_33_identity = {{1., 0., 0.,},
                                                     {0., 1., 0.},
                                                     {0., 0., 1.}};
static constexpr cs_real_6_t cs_math_sym_33_identity  = {1., 1., 1.,
                                                         0. ,0., 0.};
 
/*----------------------------------------------------------------------------*/
 
END_C_DECLS
 
/*=============================================================================
 * Templated inline functions
 *============================================================================*/
 
#if defined(__cplusplus)
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE inline T
cs_math_sq(T  x)
{
  return x*x;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U>
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_distance_dot_product(const T  xa[3],
                               const T  xb[3],
                               const U  xc[3])
{
  return ((xb[0] - xa[0])*xc[0]+(xb[1] - xa[1])*xc[1]+(xb[2] - xa[2])*xc[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U>
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_dot_product(const T  u[3],
                      const U  v[3])
{
  double uv = u[0]*v[0] + u[1]*v[1] + u[2]*v[2];
 
  return uv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE inline T
cs_math_3_dot_product(const T  u[3],
                      const T  v[3])
{
  T uv = u[0]*v[0] + u[1]*v[1] + u[2]*v[2];
 
  return uv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE inline T
cs_math_3_norm(const T  v[3])
{
  return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE inline T
cs_math_33_trace(const T  t[3][3])
{
  return (t[0][0] + t[1][1] + t[2][2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_sym_33_3_dot_product(const T  n1[3],
                               const U  t[6],
                               const V  n2[3])
{
  return (  n1[0] * (t[0]*n2[0] + t[3]*n2[1] + t[5]*n2[2])
          + n1[1] * (t[3]*n2[0] + t[1]*n2[1] + t[4]*n2[2])
          + n1[2] * (t[5]*n2[0] + t[4]*n2[1] + t[2]*n2[2]));
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <cs_lnum_t stride>
CS_F_HOST_DEVICE inline cs_real_t
cs_math_square_norm(const cs_real_t  t[stride])
{
  cs_real_t retval;
 
  if (stride == 3)
    retval = t[0]*t[0] + t[1]*t[1] + t[2]*t[2];
  else if (stride == 6)
    retval =     t[0]*t[0] +   t[1]*t[1] +   t[2]*t[2]
             + 2*t[3]*t[3] + 2*t[4]*t[4] + 2*t[5]*t[5];
  else
    retval = 1;
 
  return retval;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE inline T
cs_math_3_square_norm(const T v[3])
{
  cs_real_t v2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
 
  return v2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U>
CS_F_HOST_DEVICE inline void
cs_math_3_normalize(const T  vin[3],
                    U        vout[3])
{
  cs_real_t norm = cs_math_3_norm(vin);
 
  cs_real_t inv_norm = ((norm > cs_math_zero_threshold) ?  1. / norm : 0);
 
  vout[0] = inv_norm * vin[0];
  vout[1] = inv_norm * vin[1];
  vout[2] = inv_norm * vin[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline void
cs_math_33_3_product(const T      m[3][3],
                     const U      v[3],
                     V  *restrict mv)
{
  mv[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
  mv[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
  mv[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline void
cs_math_sym_33_3_product(const T       m[6],
                         const U       v[3],
                         V  *restrict  mv)
{
  mv[0] = m[0]*v[0] + m[3]*v[1] + m[5]*v[2];
  mv[1] = m[3]*v[0] + m[1]*v[1] + m[4]*v[2];
  mv[2] = m[5]*v[0] + m[4]*v[1] + m[2]*v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline void
cs_math_3_normal_scaling(const T  n[3],
                         U        factor,
                         V        v[3])
{
  cs_real_t v_dot_n = (factor -1.) * cs_math_3_dot_product(v, n);
  for (int i = 0; i < 3; i++)
    v[i] += v_dot_n * n[i];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_33_3_dot_product(const T  n1[3],
                           const U  t[3][3],
                           const V  n2[3])
{
  cs_real_t n_t_n
    = (  n1[0]*t[0][0]*n2[0] + n1[1]*t[1][0]*n2[0] + n1[2]*t[2][0]*n2[0]
       + n1[0]*t[0][1]*n2[1] + n1[1]*t[1][1]*n2[1] + n1[2]*t[2][1]*n2[1]
       + n1[0]*t[0][2]*n2[2] + n1[1]*t[1][2]*n2[2] + n1[2]*t[2][2]*n2[2]);
  return n_t_n;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline void
cs_math_3_orthogonal_projection(const T      n[3],
                                const U      v[3],
                                V  *restrict vout)
{
  vout[0] =  v[0]*(1.-n[0]*n[0])- v[1]*    n[1]*n[0] - v[2]*    n[2]*n[0];
  vout[1] = -v[0]*    n[0]*n[1] + v[1]*(1.-n[1]*n[1])- v[2]*    n[2]*n[1];
  vout[2] = -v[0]*    n[0]*n[2] - v[1]*    n[1]*n[2] + v[2]*(1.-n[2]*n[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T, typename U, typename V>
CS_F_HOST_DEVICE inline void
cs_math_3_cross_product(const T       u[3],
                        const U       v[3],
                        V  *restrict  uv)
{
  uv[0] = u[1]*v[2] - u[2]*v[1];
  uv[1] = u[2]*v[0] - u[0]*v[2];
  uv[2] = u[0]*v[1] - u[1]*v[0];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <cs_lnum_t n, typename T>
CS_F_HOST_DEVICE inline void
cs_math_matrix_gauss_inverse(const T  a[n][n],
                             T        b[n][n])
{
  T *p_a[n];
  T t[n][n*2];
 
  for (size_t i = 0; i < n; i++) {
    p_a[i] = t[i];
    for (size_t j = 0; j < n; j++) {
      p_a[i][j] = a[i][j];
      p_a[i][j+n] = 0;
    }
    p_a[i][i+n] = 1;
  }
 
  for (size_t i = 0; i < n; i++) {
 
    // Pivot matrix rows (swap pointers)
    for (size_t j = i+1; j < n; j++) {
      if (cs::abs(p_a[i][i]) < cs::abs(p_a[j][i])) {
        T* tmp = p_a[j];
        p_a[j] = p_a[i];
        p_a[i] = tmp;
      }
    }
    // Linear combination of each row with other rows
    for (size_t j = 0; j < n; j++) {
      if (j != i) {
        T s = p_a[j][i] / p_a[i][i];
        for (size_t k = 0; k < 2*n; k++) {
          p_a[j][k] -= p_a[i][k] * s;
        }
      }
    }
 
  }
 
  // Scale and copy results
  for (size_t i = 0; i < n; i++) {
    T s = (T)1. / p_a[i][i];
    for (size_t j = 0; j < n; j++) {
      b[i][j] = p_a[i][n+j] * s;
    }
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
template <typename T>
CS_F_HOST_DEVICE void
cs_math_6_gauss_inverse_in_place(T  a[6])
{
  T t[3][3] = {{a[0], a[3], a[5]},
               {a[3], a[1], a[4]},
               {a[5], a[4], a[2]}};
  T s[3][3];
 
  cs_math_matrix_gauss_inverse(t, s);
 
  a[0] = s[0][0];
  a[1] = s[1][1];
  a[2] = s[2][2];
 
  a[3] = s[0][1];
  a[4] = s[1][2];
  a[5] = s[0][2];
}
 
namespace cs {
 
/*----------------------------------------------------------------------------*/
/*
 * \brief  Return absolute value of a number.
 *
 * Specialized versions for floating point numbers should be faster than
 * base version from cs_defs.h.
 *
 * \param[in]  a  value
 *
 * \return absolute value of a
 */
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline double
abs(const double  a)
{
  return fabs(a);
}
 
CS_F_HOST_DEVICE inline float
abs(const float  a)
{
  return fabsf(a);
}
 
/*----------------------------------------------------------------------------*/
/*
 * \brief  Return minimum of two values.
 *
 * Specialized versions for floating point numbers should be faster than
 * base version from cs_defs.h.
 *
 * \param[in]  a   first value
 * \param[in]  b   second value
 *
 * \return minimum of a and b
 */
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline double
min(const double  a,
    const double  b)
{
  return fmin(a, b);
}
 
CS_F_HOST_DEVICE inline float
min(const float  a,
    const float  b)
{
  return fminf(a, b);
}
 
/*----------------------------------------------------------------------------*/
/*
 * \brief  Return maximum of two values.
 *
 * Specialized versions for floating point numbers should be faster than
 * base version from cs_defs.h.
 *
 * \param[in]  a   first value
 * \param[in]  b   second value
 *
 * \return maximum of a and b
 */
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline double
max(const double  a,
    const double  b)
{
  return fmax(a, b);
}
 
CS_F_HOST_DEVICE inline float
max(const float  a,
    const float  b)
{
  return fmaxf(a, b);
}
 
/*----------------------------------------------------------------------------*/
/*
 * \brief Clamp function for a given scalar value.
 *
 * Specialized versions for floating point numbers should be faster than
 * base version from cs_defs.h.
 *
 * \param[in] x    initial value
 * \param[in] xmin min value for clamping
 * \param[in] xmax max value for clamping
 *
 * \return clamped value which is 'x' if xmin < x < xmax or lower/upper bound
 * otherwise
 */
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline double
clamp(const double x,
      const double xmin,
      const double xmax)
{
  return fmin(xmax, fmax(xmin, x));
}
 
CS_F_HOST_DEVICE inline float
clamp(const float x,
      const float xmin,
      const float xmax)
{
  return fminf(xmax, fmaxf(xmin, x));
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline float
pow2(float  x)
{
  return x*x;
}
 
CS_F_HOST_DEVICE inline double
pow2(double  x)
{
  return x*x;
}
 
} // namespace cs
 
#endif // defined(__cplusplus)
 
BEGIN_C_DECLS
 
/*=============================================================================
 * Inline static functions
 *============================================================================*/
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
inline int
cs_math_binom(int  n,
              int  k)
{
  int ret = 1;
  assert(n >= k);
 
  const int n_iter = (k > n-k) ? n-k : k;
  for (int j = 1; j <= n_iter; j++, n--) {
    if (n % j == 0)
      ret *= n/j;
    else if (ret % j == 0)
      ret = ret/j*n;
    else
      ret = (ret*n)/j;
  }
 
  return ret;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_fabs(cs_real_t  x)
{
  cs_real_t ret = (x <  0) ? -x : x;
 
  return ret;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_fmin(cs_real_t  x,
             cs_real_t  y)
{
  cs_real_t ret = (x <  y) ? x : y;
 
  return ret;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_fmax(cs_real_t  x,
             cs_real_t  y)
{
  cs_real_t ret = (x <  y) ? y : x;
 
  return ret;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_clamp(cs_real_t x,
              cs_real_t xmin,
              cs_real_t xmax)
{
  cs_real_t ret = cs_math_fmin(xmax, cs_math_fmax(xmin, x));
 
  return ret;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_sq(cs_real_t  x)
{
  return x*x;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_pow2(cs_real_t  x)
{
  return x*x;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_pow3(cs_real_t  x)
{
  return x*x*x;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_pow4(cs_real_t  x)
{
  cs_real_t x2 = x * x;
  return x2 * x2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_pow5(cs_real_t  x)
{
  cs_real_t x2 = x * x;
  return x * x2 * x2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_distance(const cs_real_t  xa[3],
                   const cs_real_t  xb[3])
{
  cs_real_t  v[3];
 
  v[0] = xb[0] - xa[0];
  v[1] = xb[1] - xa[1];
  v[2] = xb[2] - xa[2];
 
  return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  return ((xb[0] - xa[0])*xc[0]+(xb[1] - xa[1])*xc[1]+(xb[2] - xa[2])*xc[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_square_distance(const cs_real_t  xa[3],
                          const cs_real_t  xb[3])
{
  cs_real_t v[3] = {xb[0] - xa[0],
                    xb[1] - xa[1],
                    xb[2] - xa[2]};
 
  return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_dot_product(const cs_real_t  u[3],
                      const cs_real_t  v[3])
{
  cs_real_t uv = u[0]*v[0] + u[1]*v[1] + u[2]*v[2];
 
  return uv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  cs_real_t n_t_n
    = (  n1[0]*t[0][0]*n2[0] + n1[1]*t[1][0]*n2[0] + n1[2]*t[2][0]*n2[0]
       + n1[0]*t[0][1]*n2[1] + n1[1]*t[1][1]*n2[1] + n1[2]*t[2][1]*n2[1]
       + n1[0]*t[0][2]*n2[2] + n1[1]*t[1][2]*n2[2] + n1[2]*t[2][2]*n2[2]);
  return n_t_n;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  return (  n1[0] * (t[0]*n2[0] + t[3]*n2[1] + t[5]*n2[2])
          + n1[1] * (t[3]*n2[0] + t[1]*n2[1] + t[4]*n2[2])
          + n1[2] * (t[5]*n2[0] + t[4]*n2[1] + t[2]*n2[2]));
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_norm(const cs_real_t  v[3])
{
  return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_3_square_norm(const cs_real_t v[3])
{
  cs_real_t v2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
 
  return v2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_3_normalize(const cs_real_t  vin[3],
                    cs_real_t        vout[3])
{
  cs_real_t norm = cs_math_3_norm(vin);
 
  cs_real_t inv_norm = ((norm > cs_math_zero_threshold) ?  1. / norm : 0);
 
  vout[0] = inv_norm * vin[0];
  vout[1] = inv_norm * vin[1];
  vout[2] = inv_norm * vin[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_3_normalize_threshold(const cs_real_t vin[3],
                              const cs_real_t thres,
                              cs_real_t       vout[3])
{
  cs_real_t norm = cs_math_3_norm(vin);
 
  cs_real_t inv_norm = ((norm > thres) ? 1. / norm : 1. / thres);
 
  vout[0] = inv_norm * vin[0];
  vout[1] = inv_norm * vin[1];
  vout[2] = inv_norm * vin[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_3_orthogonal_projection(const cs_real_t      n[3],
                                const cs_real_t      v[3],
                                cs_real_t  *restrict vout)
{
  vout[0] =  v[0]*(1.-n[0]*n[0])- v[1]*    n[1]*n[0] - v[2]*    n[2]*n[0];
  vout[1] = -v[0]*    n[0]*n[1] + v[1]*(1.-n[1]*n[1])- v[2]*    n[2]*n[1];
  vout[2] = -v[0]*    n[0]*n[2] - v[1]*    n[1]*n[2] + v[2]*(1.-n[2]*n[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_normal_scaling_add(const cs_real_t  n[3],
                              cs_real_t        factor,
                              cs_real_t        t[3][3])
{
  cs_real_t n_t_n = (factor -1.) *
    ( n[0] * t[0][0] * n[0] + n[1] * t[1][0] * n[0] + n[2] * t[2][0] * n[0]
    + n[0] * t[0][1] * n[1] + n[1] * t[1][1] * n[1] + n[2] * t[2][1] * n[1]
    + n[0] * t[0][2] * n[2] + n[1] * t[1][2] * n[2] + n[2] * t[2][2] * n[2]);
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++)
      t[i][j] += n_t_n * n[i] * n[j];
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_3_product_add(const cs_real_t       m[3][3],
                         const cs_real_t       v[3],
                         cs_real_t  *restrict  mv)
{
  mv[0] += m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
  mv[1] += m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
  mv[2] += m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33t_3_product(const cs_real_t       m[3][3],
                      const cs_real_t       v[3],
                      cs_real_t  *restrict  mv)
{
  mv[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
  mv[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
  mv[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_3_product(const cs_real_t       m[6],
                         const cs_real_t       v[3],
                         cs_real_t  *restrict  mv)
{
  mv[0] = m[0]*v[0] + m[3]*v[1] + m[5]*v[2];
  mv[1] = m[3]*v[0] + m[1]*v[1] + m[4]*v[2];
  mv[2] = m[5]*v[0] + m[4]*v[1] + m[2]*v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_3_product_add(const cs_real_t       m[6],
                             const cs_real_t       v[3],
                             cs_real_t  *restrict  mv)
{
  mv[0] += m[0] * v[0] + m[3] * v[1] + m[5] * v[2];
  mv[1] += m[3] * v[0] + m[1] * v[1] + m[4] * v[2];
  mv[2] += m[5] * v[0] + m[4] * v[1] + m[2] * v[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_sym_33_sym_33_product_trace(const cs_real_t  m1[6],
                                    const cs_real_t  m2[6])
{
  return m1[0]*m2[0] + 2.*m1[3]*m2[3] + 2.*m1[5]*m2[5]
                     +    m1[1]*m2[1] + 2.*m1[4]*m2[4]
                                      +    m1[2]*m2[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_6_trace(const cs_real_t  t[6])
{
  return (t[0] + t[1] + t[2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_66_6_product(const cs_real_t       m[6][6],
                     const cs_real_t       v[6],
                     cs_real_t  *restrict  mv)
{
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 6; j++)
      mv[i] = m[i][j] * v[j];
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_66_6_product_add(const cs_real_t       m[6][6],
                         const cs_real_t       v[6],
                         cs_real_t  *restrict  mv)
{
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 6; j++)
      mv[i] += m[i][j] * v[j];
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_33_determinant(const cs_real_t   m[3][3])
{
  const cs_real_t  com0 = m[1][1]*m[2][2] - m[2][1]*m[1][2];
  const cs_real_t  com1 = m[2][1]*m[0][2] - m[0][1]*m[2][2];
  const cs_real_t  com2 = m[0][1]*m[1][2] - m[1][1]*m[0][2];
 
  return m[0][0]*com0 + m[1][0]*com1 + m[2][0]*com2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_sym_33_determinant(const cs_real_t  m[6])
{
  const cs_real_t  com0 = m[1]*m[2] - m[4]*m[4];
  const cs_real_t  com1 = m[4]*m[5] - m[3]*m[2];
  const cs_real_t  com2 = m[3]*m[4] - m[1]*m[5];
 
  return m[0]*com0 + m[3]*com1 + m[5]*com2;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_3_average(const cs_real_t       u[3],
                  const cs_real_t       v[3],
                  cs_real_t  *restrict  uv)
{
  uv[0] = (u[0] + v[0]) / 2.0;
  uv[1] = (u[1] + v[1]) / 2.0;
  uv[2] = (u[2] + v[2]) / 2.0;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
#if defined(__INTEL_COMPILER)
#pragma optimization_level 0 /* Bug with O1 or above with icc 15.0.1 20141023 */
#endif
 
CS_F_HOST_DEVICE inline void
cs_math_3_cross_product(const cs_real_t       u[3],
                        const cs_real_t       v[3],
                        cs_real_t  *restrict  uv)
{
  uv[0] = u[1]*v[2] - u[2]*v[1];
  uv[1] = u[2]*v[0] - u[0]*v[2];
  uv[2] = u[0]*v[1] - u[1]*v[0];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
#if defined(__INTEL_COMPILER)
#pragma optimization_level 0 /* Bug with O1 or above with icc 15.0.1 20141023 */
#endif
 
CS_F_HOST_DEVICE 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])
{
  return    (u[1]*v[2] - u[2]*v[1]) * w[0]
          + (u[2]*v[0] - u[0]*v[2]) * w[1]
          + (u[0]*v[1] - u[1]*v[0]) * w[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_3_orthonormal_basis(const cs_real_t vect[3],
                            cs_real_t       axes[3][3])
{
  assert(cs_math_3_norm(vect) > cs_math_zero_threshold);
 
  // Compute normal - third axis
  cs_math_3_normalize(vect, axes[2]);
 
  // Compute base's first axis
  // First test projection of Ox
  cs_real_t Ox[3] = {1., 0., 0.};
  cs_real_t w[3] = {0.};
 
  cs_math_3_orthogonal_projection(axes[2], Ox, w);
 
  // If Ox projection is null, project Oy
  if (cs_math_3_norm(w) < cs_math_zero_threshold) {
    cs_real_t Oy[3] = {0., 1., 0.};
    cs_math_3_orthogonal_projection(axes[2], Oy, w);
  }
 
  cs_math_3_normalize(w, axes[0]);
 
  // Compute base's second axis using cross product
  cs_math_3_cross_product(axes[2], axes[0], axes[1]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_inv_cramer(const cs_real_t  in[3][3],
                      cs_real_t        out[3][3])
{
  out[0][0] = in[1][1]*in[2][2] - in[2][1]*in[1][2];
  out[0][1] = in[2][1]*in[0][2] - in[0][1]*in[2][2];
  out[0][2] = in[0][1]*in[1][2] - in[1][1]*in[0][2];
 
  out[1][0] = in[2][0]*in[1][2] - in[1][0]*in[2][2];
  out[1][1] = in[0][0]*in[2][2] - in[2][0]*in[0][2];
  out[1][2] = in[1][0]*in[0][2] - in[0][0]*in[1][2];
 
  out[2][0] = in[1][0]*in[2][1] - in[2][0]*in[1][1];
  out[2][1] = in[2][0]*in[0][1] - in[0][0]*in[2][1];
  out[2][2] = in[0][0]*in[1][1] - in[1][0]*in[0][1];
 
  const double  det = in[0][0]*out[0][0]+in[1][0]*out[0][1]+in[2][0]*out[0][2];
  const double  invdet = 1./det;
 
  out[0][0] *= invdet, out[0][1] *= invdet, out[0][2] *= invdet;
  out[1][0] *= invdet, out[1][1] *= invdet, out[1][2] *= invdet;
  out[2][0] *= invdet, out[2][1] *= invdet, out[2][2] *= invdet;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_inv_cramer_in_place(cs_real_t  a[3][3])
{
  cs_real_t a00 = a[1][1]*a[2][2] - a[2][1]*a[1][2];
  cs_real_t a01 = a[2][1]*a[0][2] - a[0][1]*a[2][2];
  cs_real_t a02 = a[0][1]*a[1][2] - a[1][1]*a[0][2];
  cs_real_t a10 = a[2][0]*a[1][2] - a[1][0]*a[2][2];
  cs_real_t a11 = a[0][0]*a[2][2] - a[2][0]*a[0][2];
  cs_real_t a12 = a[1][0]*a[0][2] - a[0][0]*a[1][2];
  cs_real_t a20 = a[1][0]*a[2][1] - a[2][0]*a[1][1];
  cs_real_t a21 = a[2][0]*a[0][1] - a[0][0]*a[2][1];
  cs_real_t a22 = a[0][0]*a[1][1] - a[1][0]*a[0][1];
 
  double det_inv = 1. / (a[0][0]*a00 + a[1][0]*a01 + a[2][0]*a02);
 
  a[0][0] = a00 * det_inv;
  a[0][1] = a01 * det_inv;
  a[0][2] = a02 * det_inv;
  a[1][0] = a10 * det_inv;
  a[1][1] = a11 * det_inv;
  a[1][2] = a12 * det_inv;
  a[2][0] = a20 * det_inv;
  a[2][1] = a21 * det_inv;
  a[2][2] = a22 * det_inv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_inv_cramer_sym_in_place(cs_real_t  a[3][3])
{
  cs_real_t a00 = a[1][1]*a[2][2] - a[2][1]*a[1][2];
  cs_real_t a01 = a[2][1]*a[0][2] - a[0][1]*a[2][2];
  cs_real_t a02 = a[0][1]*a[1][2] - a[1][1]*a[0][2];
  cs_real_t a11 = a[0][0]*a[2][2] - a[2][0]*a[0][2];
  cs_real_t a12 = a[1][0]*a[0][2] - a[0][0]*a[1][2];
  cs_real_t a22 = a[0][0]*a[1][1] - a[1][0]*a[0][1];
 
  double det_inv = 1. / (a[0][0]*a00 + a[1][0]*a01 + a[2][0]*a02);
 
  a[0][0] = a00 * det_inv;
  a[0][1] = a01 * det_inv;
  a[0][2] = a02 * det_inv;
  a[1][0] = a01 * det_inv;
  a[1][1] = a11 * det_inv;
  a[1][2] = a12 * det_inv;
  a[2][0] = a02 * det_inv;
  a[2][1] = a12 * det_inv;
  a[2][2] = a22 * det_inv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_inv_cramer(const cs_real_t       s[6],
                          cs_real_t  *restrict  sout)
{
  double detinv;
 
  sout[0] = s[1]*s[2] - s[4]*s[4];
  sout[1] = s[0]*s[2] - s[5]*s[5];
  sout[2] = s[0]*s[1] - s[3]*s[3];
  sout[3] = s[4]*s[5] - s[3]*s[2];
  sout[4] = s[3]*s[5] - s[0]*s[4];
  sout[5] = s[3]*s[4] - s[1]*s[5];
 
  detinv = 1. / (s[0]*sout[0] + s[3]*sout[3] + s[5]*sout[5]);
 
  sout[0] *= detinv;
  sout[1] *= detinv;
  sout[2] *= detinv;
  sout[3] *= detinv;
  sout[4] *= detinv;
  sout[5] *= detinv;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  mout[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
  mout[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
  mout[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
 
  mout[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
  mout[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
  mout[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
 
  mout[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
  mout[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
  mout[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  /* _m = M.Q */
  cs_real_33_t _m;
  _m[0][0] = m[0][0]*q[0][0] + m[0][1]*q[1][0] + m[0][2]*q[2][0];
  _m[0][1] = m[0][0]*q[0][1] + m[0][1]*q[1][1] + m[0][2]*q[2][1];
  _m[0][2] = m[0][0]*q[0][2] + m[0][1]*q[1][2] + m[0][2]*q[2][2];
 
  _m[1][0] = m[1][0]*q[0][0] + m[1][1]*q[1][0] + m[1][2]*q[2][0];
  _m[1][1] = m[1][0]*q[0][1] + m[1][1]*q[1][1] + m[1][2]*q[2][1];
  _m[1][2] = m[1][0]*q[0][2] + m[1][1]*q[1][2] + m[1][2]*q[2][2];
 
  _m[2][0] = m[2][0]*q[0][0] + m[2][1]*q[1][0] + m[2][2]*q[2][0];
  _m[2][1] = m[2][0]*q[0][1] + m[2][1]*q[1][1] + m[2][2]*q[2][1];
  _m[2][2] = m[2][0]*q[0][2] + m[2][1]*q[1][2] + m[2][2]*q[2][2];
 
  /* mout = Q^t _m */
  mout[0][0] = q[0][0]*_m[0][0] + q[1][0]*_m[1][0] + q[2][0]*_m[2][0];
  mout[0][1] = q[0][0]*_m[0][1] + q[1][0]*_m[1][1] + q[2][0]*_m[2][1];
  mout[0][2] = q[0][0]*_m[0][2] + q[1][0]*_m[1][2] + q[2][0]*_m[2][2];
 
  mout[1][0] = q[0][1]*_m[0][0] + q[1][1]*_m[1][0] + q[2][1]*_m[2][0];
  mout[1][1] = q[0][1]*_m[0][1] + q[1][1]*_m[1][1] + q[2][1]*_m[2][1];
  mout[1][2] = q[0][1]*_m[0][2] + q[1][1]*_m[1][2] + q[2][1]*_m[2][2];
 
  mout[2][0] = q[0][2]*_m[0][0] + q[1][2]*_m[1][0] + q[2][2]*_m[2][0];
  mout[2][1] = q[0][2]*_m[0][1] + q[1][2]*_m[1][1] + q[2][2]*_m[2][1];
  mout[2][2] = q[0][2]*_m[0][2] + q[1][2]*_m[1][2] + q[2][2]*_m[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_transform_r_to_a(const cs_real_t  m[6],
                                const cs_real_t  q[3][3],
                                cs_real_t        mout[6])
{
  /* _m = M.Q */
  cs_real_33_t _m;
  _m[0][0] = m[0]*q[0][0] + m[3]*q[1][0] + m[5]*q[2][0];
  _m[0][1] = m[0]*q[0][1] + m[3]*q[1][1] + m[5]*q[2][1];
  _m[0][2] = m[0]*q[0][2] + m[3]*q[1][2] + m[5]*q[2][2];
 
  _m[1][0] = m[3]*q[0][0] + m[1]*q[1][0] + m[4]*q[2][0];
  _m[1][1] = m[3]*q[0][1] + m[1]*q[1][1] + m[4]*q[2][1];
  _m[1][2] = m[3]*q[0][2] + m[1]*q[1][2] + m[4]*q[2][2];
 
  _m[2][0] = m[5]*q[0][0] + m[4]*q[1][0] + m[2]*q[2][0];
  _m[2][1] = m[5]*q[0][1] + m[4]*q[1][1] + m[2]*q[2][1];
  _m[2][2] = m[5]*q[0][2] + m[4]*q[1][2] + m[2]*q[2][2];
 
  /* mout = Q^t _m */
  mout[0] = q[0][0]*_m[0][0] + q[1][0]*_m[1][0] + q[2][0]*_m[2][0];
  mout[1] = q[0][1]*_m[0][1] + q[1][1]*_m[1][1] + q[2][1]*_m[2][1];
  mout[2] = q[0][2]*_m[0][2] + q[1][2]*_m[1][2] + q[2][2]*_m[2][2];
 
  mout[3] = q[0][0]*_m[0][1] + q[1][0]*_m[1][1] + q[2][0]*_m[2][1];
  mout[4] = q[0][1]*_m[0][2] + q[1][1]*_m[1][2] + q[2][1]*_m[2][2];
  mout[5] = q[0][0]*_m[0][2] + q[1][0]*_m[1][2] + q[2][0]*_m[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  /* _m = M.Q^t */
  cs_real_33_t _m;
  _m[0][0] = m[0][0]*q[0][0] + m[0][1]*q[0][1] + m[0][2]*q[0][2];
  _m[0][1] = m[0][0]*q[1][0] + m[0][1]*q[1][1] + m[0][2]*q[1][2];
  _m[0][2] = m[0][0]*q[2][0] + m[0][1]*q[2][1] + m[0][2]*q[2][2];
 
  _m[1][0] = m[1][0]*q[0][0] + m[1][1]*q[0][1] + m[1][2]*q[0][2];
  _m[1][1] = m[1][0]*q[1][0] + m[1][1]*q[1][1] + m[1][2]*q[1][2];
  _m[1][2] = m[1][0]*q[2][0] + m[1][1]*q[2][1] + m[1][2]*q[2][2];
 
  _m[2][0] = m[2][0]*q[0][0] + m[2][1]*q[0][1] + m[2][2]*q[0][2];
  _m[2][1] = m[2][0]*q[1][0] + m[2][1]*q[1][1] + m[2][2]*q[1][2];
  _m[2][2] = m[2][0]*q[2][0] + m[2][1]*q[2][1] + m[2][2]*q[2][2];
 
  /* mout = Q _m */
  mout[0][0] = q[0][0]*_m[0][0] + q[0][1]*_m[1][0] + q[0][2]*_m[2][0];
  mout[0][1] = q[0][0]*_m[0][1] + q[0][1]*_m[1][1] + q[0][2]*_m[2][1];
  mout[0][2] = q[0][0]*_m[0][2] + q[0][1]*_m[1][2] + q[0][2]*_m[2][2];
 
  mout[1][0] = q[1][0]*_m[0][0] + q[1][1]*_m[1][0] + q[1][2]*_m[2][0];
  mout[1][1] = q[1][0]*_m[0][1] + q[1][1]*_m[1][1] + q[1][2]*_m[2][1];
  mout[1][2] = q[1][0]*_m[0][2] + q[1][1]*_m[1][2] + q[1][2]*_m[2][2];
 
  mout[2][0] = q[2][0]*_m[0][0] + q[2][1]*_m[1][0] + q[2][2]*_m[2][0];
  mout[2][1] = q[2][0]*_m[0][1] + q[2][1]*_m[1][1] + q[2][2]*_m[2][1];
  mout[2][2] = q[2][0]*_m[0][2] + q[2][1]*_m[1][2] + q[2][2]*_m[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_transform_a_to_r(const cs_real_t  m[6],
                                const cs_real_t  q[3][3],
                                cs_real_t        mout[6])
{
  /* _m = M.Q^t */
  cs_real_33_t _m;
  _m[0][0] = m[0]*q[0][0] + m[3]*q[0][1] + m[5]*q[0][2];
  _m[0][1] = m[0]*q[1][0] + m[3]*q[1][1] + m[5]*q[1][2];
  _m[0][2] = m[0]*q[2][0] + m[3]*q[2][1] + m[5]*q[2][2];
 
  _m[1][0] = m[3]*q[0][0] + m[1]*q[0][1] + m[4]*q[0][2];
  _m[1][1] = m[3]*q[1][0] + m[1]*q[1][1] + m[4]*q[1][2];
  _m[1][2] = m[3]*q[2][0] + m[1]*q[2][1] + m[4]*q[2][2];
 
  _m[2][0] = m[5]*q[0][0] + m[4]*q[0][1] + m[2]*q[0][2];
  _m[2][1] = m[5]*q[1][0] + m[4]*q[1][1] + m[2]*q[1][2];
  _m[2][2] = m[5]*q[2][0] + m[4]*q[2][1] + m[2]*q[2][2];
 
  /* mout = Q _m */
  mout[0] = q[0][0]*_m[0][0] + q[0][1]*_m[1][0] + q[0][2]*_m[2][0];
  mout[1] = q[1][0]*_m[0][1] + q[1][1]*_m[1][1] + q[1][2]*_m[2][1];
  mout[2] = q[2][0]*_m[0][2] + q[2][1]*_m[1][2] + q[2][2]*_m[2][2];
 
 
  mout[3] = q[0][0]*_m[0][1] + q[0][1]*_m[1][1] + q[0][2]*_m[2][1];
  mout[4] = q[1][0]*_m[0][2] + q[1][1]*_m[1][2] + q[1][2]*_m[2][2];
  mout[5] = q[0][0]*_m[0][2] + q[0][1]*_m[1][2] + q[0][2]*_m[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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])
{
  /* sym = 0.5 (m + m_transpose) */
  m_sym[0][0] = 0.5 * (m[0][0] + m[0][0]);
  m_sym[0][1] = 0.5 * (m[0][1] + m[1][0]);
  m_sym[0][2] = 0.5 * (m[0][2] + m[2][0]);
  m_sym[1][0] = 0.5 * (m[1][0] + m[0][1]);
  m_sym[1][1] = 0.5 * (m[1][1] + m[1][1]);
  m_sym[1][2] = 0.5 * (m[1][2] + m[2][1]);
  m_sym[2][0] = 0.5 * (m[2][0] + m[0][2]);
  m_sym[2][1] = 0.5 * (m[2][1] + m[1][2]);
  m_sym[2][2] = 0.5 * (m[2][2] + m[2][2]);
 
  /* ant = 0.5 (m - m_transpose) */
  m_ant[0][0] = 0.5 * (m[0][0] - m[0][0]);
  m_ant[0][1] = 0.5 * (m[0][1] - m[1][0]);
  m_ant[0][2] = 0.5 * (m[0][2] - m[2][0]);
  m_ant[1][0] = 0.5 * (m[1][0] - m[0][1]);
  m_ant[1][1] = 0.5 * (m[1][1] - m[1][1]);
  m_ant[1][2] = 0.5 * (m[1][2] - m[2][1]);
  m_ant[2][0] = 0.5 * (m[2][0] - m[0][2]);
  m_ant[2][1] = 0.5 * (m[2][1] - m[1][2]);
  m_ant[2][2] = 0.5 * (m[2][2] - m[2][2]);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_33_main_invariant_2(const cs_real_t  m[3][3])
{
  /* sym = 0.5 (m + m_transpose) */
  return cs_math_pow2(m[0][0])
       + cs_math_pow2(m[1][1])
       + cs_math_pow2(m[2][2])
    + 0.5 * ( cs_math_pow2(m[0][1] + m[1][0])
            + cs_math_pow2(m[0][2] + m[2][0])
            + cs_math_pow2(m[1][2] + m[2][1]));
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_product_add(const cs_real_t             m1[3][3],
                       const cs_real_t             m2[3][3],
                       cs_real_t        (*restrict mout)[3])
{
  mout[0][0] += m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
  mout[0][1] += m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
  mout[0][2] += m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
 
  mout[1][0] += m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
  mout[1][1] += m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
  mout[1][2] += m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
 
  mout[2][0] += m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
  mout[2][1] += m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
  mout[2][2] += m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_33_product(const cs_real_t      s1[6],
                       const cs_real_t      s2[6],
                       cs_real_t *restrict  sout)
{
  /* S11 */
  sout[0] = s1[0]*s2[0] + s1[3]*s2[3] + s1[5]*s2[5];
  /* S22 */
  sout[1] = s1[3]*s2[3] + s1[1]*s2[1] + s1[4]*s2[4];
  /* S33 */
  sout[2] = s1[5]*s2[5] + s1[4]*s2[4] + s1[2]*s2[2];
  /* S12 = S21 */
  sout[3] = s1[0]*s2[3] + s1[3]*s2[1] + s1[5]*s2[4];
  /* S23 = S32 */
  sout[4] = s1[3]*s2[5] + s1[1]*s2[4] + s1[4]*s2[2];
  /* S13 = S31 */
  sout[5] = s1[0]*s2[5] + s1[3]*s2[4] + s1[5]*s2[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_33_rotation_into_quaternions(const cs_real_t       m[3][3],
                                     cs_real_t             e[4])
{
  /* Distinguish 4 different cases to avoid division by 0 */
  if (fabs(1.+m[0][0]+m[1][1]+m[2][2]) > 1.0e-12) {
    // Case 1: e[0] is not equal to 0
    e[0] = 0.5*sqrt(1+m[0][0]+m[1][1]+m[2][2]);
    e[1] = 0.25*(m[2][1]-m[1][2])/e[0];
    e[2] = 0.25*(m[0][2]-m[2][0])/e[0];
    e[3] = 0.25*(m[1][0]-m[0][1])/e[0];
  }
  else if (fabs(1+m[0][0]-m[1][1]-m[2][2]) > 1.0e-12) {
    // Case 2: e[1] is not equal to 0
    e[1] = 0.5*sqrt(1+m[0][0]-m[1][1]-m[2][2]);
    e[0] = 0.25*(m[2][1]-m[1][2])/e[1];
    e[2] = 0.25*(m[1][0]+m[0][1])/e[1];
    e[3] = 0.25*(m[0][2]+m[2][0])/e[1];
  }
  else if (fabs(1-m[0][0]+m[1][1]-m[2][2]) > 1.0e-12) {
    // Case 3: e[2] is not equal to 0
    e[2] = 0.5*sqrt(1-m[0][0]+m[1][1]-m[2][2]);
    e[0] = 0.25*(m[0][2]-m[2][0])/e[2];
    e[1] = 0.25*(m[1][0]+m[0][1])/e[2];
    e[3] = 0.25*(m[2][1]+m[1][2])/e[2];
  }
  else if (fabs(1-m[0][0]-m[1][1]+m[2][2]) > 1.0e-12) {
    // Case 4: e[3] is not equal to 0
    e[3] = 0.5*sqrt(1-m[0][0]-m[1][1]+m[2][2]);
    e[0] = 0.25*(m[1][0]-m[0][1])/e[3];
    e[1] = 0.25*(m[0][2]+m[2][0])/e[3];
    e[2] = 0.25*(m[2][1]+m[1][2])/e[3];
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_quaternions_into_33_rotation(const cs_real_t       e[4],
                                     cs_real_t             m[3][3])
{
  m[0][0] = 2.*(e[0]*e[0]+e[1]*e[1]-0.5);
  m[0][1] = 2.*(e[1]*e[2]+e[0]*e[3]);
  m[0][2] = 2.*(e[1]*e[3]-e[0]*e[2]);
  m[1][0] = 2.*(e[1]*e[2]-e[0]*e[3]);
  m[1][1] = 2.*(e[0]*e[0]+e[2]*e[2]-0.5);
  m[1][2] = 2.*(e[2]*e[3]+e[0]*e[1]);
  m[2][0] = 2.*(e[1]*e[3]+e[0]*e[2]);
  m[2][1] = 2.*(e[2]*e[3]-e[0]*e[1]);
  m[2][2] = 2.*(e[0]*e[0]+e[3]*e[3]-0.5);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_quaternions_from_2_vectors(const cs_real_t       dir[3],
                                   const cs_real_t       dir_r[3],
                                   cs_real_t             euler[4])
{
  // The rotation axis is the result of the cross product between
  // the new direction vector and the main axis.
  cs_real_t n_rot[3];
 
  // Use quaternion (cos(theta/2), sin(theta/2) n_rot)
  // where n_rot = dir ^ dir_r normalised
  // so also       dir ^ (dir + dir_r)
  //
  // cos(theta/2) = || dir + dir_r|| / 2
  cs_real_t dir_p_dir_r[3] = {dir[0] + dir_r[0],
                              dir[1] + dir_r[1],
                              dir[2] + dir_r[2]};
  cs_real_t dir_p_dir_r_normed[3];
  cs_math_3_normalize(dir_p_dir_r, dir_p_dir_r_normed);
 
  /* dir ^(dir + dir_r) / || dir + dir_r|| = sin(theta/2) n_rot
   * for the quaternion */
  cs_math_3_cross_product(dir, dir_p_dir_r_normed, n_rot);
 
  /* quaternion, could be normalized afterwards
   *
   * Note that the division seems stupid but is not
   * in case of degenerated case where dir is null
   * */
  euler[0] = cs_math_3_norm(dir_p_dir_r)
           / (cs_math_3_norm(dir) + cs_math_3_norm(dir_r));
  euler[1] = n_rot[0];
  euler[2] = n_rot[1];
  euler[3] = n_rot[2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_reduce_sym_prod_33_to_66(const cs_real_t       s[3][3],
                                 cs_real_t  (*restrict sout)[6])
{
  const int t2v[3][3] = {{0, 3, 5},
                         {3, 1, 4},
                         {5, 4, 2}};
 
  const int iv2t[6] = {0, 1, 2, 0, 1, 0};
  const int jv2t[6] = {0, 1, 2, 1, 2, 2};
 
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 6; j++)
      sout[i][j] = 0;
  }
 
  /* Consider : W = s*R + R*s^t .
   *            W_ij = Sum_{k<3} [s_ik*r_jk + s_jk*r_ik]
   * We look for A_(ij,pq) such as A*R = W
   *
   * so
   *   A_(ij,jk) takes s_ik
   * and
   *   A_(ij,ik) takes s_jk
   */
  for (int ij = 0; ij < 6; ij++) {
    int i = iv2t[ij];
    int j = jv2t[ij];
    for (int k = 0; k < 3; k++) {
      int ik = t2v[i][k];
      int jk = t2v[j][k];
 
      sout[ij][ik] += s[j][k];
      sout[ij][jk] += s[i][k];
    }
  }
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE 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  (*restrict sout)[3])
{
  cs_real_t _sout[3][3];
 
  /* S11 */
  _sout[0][0] = s1[0]*s2[0] + s1[3]*s2[3] + s1[5]*s2[5];
  /* S22 */
  _sout[1][1] = s1[3]*s2[3] + s1[1]*s2[1] + s1[4]*s2[4];
  /* S33 */
  _sout[2][2] = s1[5]*s2[5] + s1[4]*s2[4] + s1[2]*s2[2];
  /* S12  */
  _sout[0][1] = s1[0]*s2[3] + s1[3]*s2[1] + s1[5]*s2[4];
  /* S21  */
  _sout[1][0] = s2[0]*s1[3] + s2[3]*s1[1] + s2[5]*s1[4];
  /* S23  */
  _sout[1][2] = s1[3]*s2[5] + s1[1]*s2[4] + s1[4]*s2[2];
  /* S32  */
  _sout[2][1] = s2[3]*s1[5] + s2[1]*s1[4] + s2[4]*s1[2];
  /* S13  */
  _sout[0][2] = s1[0]*s2[5] + s1[3]*s2[4] + s1[5]*s2[2];
  /* S31  */
  _sout[2][0] = s2[0]*s1[5] + s2[3]*s1[4] + s2[5]*s1[2];
 
  sout[0][0] = _sout[0][0]*s3[0] + _sout[0][1]*s3[3] + _sout[0][2]*s3[5];
  /* S22 */
  sout[1][1] = _sout[1][0]*s3[3] + _sout[1][1]*s3[1] + _sout[1][2]*s3[4];
  /* S33 */
  sout[2][2] = _sout[2][0]*s3[5] + _sout[2][1]*s3[4] + _sout[2][2]*s3[2];
  /* S12  */
  sout[0][1] = _sout[0][0]*s3[3] + _sout[0][1]*s3[1] + _sout[0][2]*s3[4];
  /* S21  */
  sout[1][0] = s3[0]*_sout[1][0] + s3[3]*_sout[1][1] + s3[5]*_sout[1][2];
  /* S23  */
  sout[1][2] = _sout[1][0]*s3[5] + _sout[1][1]*s3[4] + _sout[1][2]*s3[2];
  /* S32  */
  sout[2][1] = s3[3]*_sout[2][0] + s3[1]*_sout[2][1] + s3[4]*_sout[2][2];
  /* S13  */
  sout[0][2] = _sout[0][0]*s3[5] + _sout[0][1]*s3[4] + _sout[0][2]*s3[2];
  /* S31  */
  sout[2][0] = s3[0]*_sout[2][0] + s3[3]*_sout[2][1] + s3[5]*_sout[2][2];
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_nvec3(const cs_real_t    v[3],
         cs_nvec3_t        *qv)
{
  cs_real_t  magnitude = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
 
  qv->meas = magnitude;
  if (fabs(magnitude) > cs_math_zero_threshold) {
 
    const cs_real_t  inv = 1/magnitude;
    qv->unitv[0] = inv * v[0];
    qv->unitv[1] = inv * v[1];
    qv->unitv[2] = inv * v[2];
 
  }
  else
    qv->unitv[0] = qv->unitv[1] = qv->unitv[2] = 0;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline void
cs_math_sym_44_factor_ldlt(cs_real_t  ldlt[10])
{
  /* Factorization */
 
  // j=0
  const cs_real_t  d00 = ldlt[0]; // m00
  assert(fabs(d00) > cs_math_zero_threshold);
 
  const cs_real_t  f00 = 1. / d00;
  const cs_real_t  l10 = ldlt[1] * f00; // m01
  const cs_real_t  l20 = ldlt[3] * f00; // m02
  const cs_real_t  l30 = ldlt[6] * f00; // m03
 
  // j=1
  const cs_real_t  d11 = ldlt[2] - l10*l10*d00; // m11
  assert(fabs(d11) > cs_math_zero_threshold);
  const cs_real_t  f11 = 1. / d11;
  const cs_real_t  l21 = (ldlt[4] - l20*d00*l10) * f11; // m12
  const cs_real_t  l31 = (ldlt[7] - l30*d00*l10) * f11; // m13
 
  // j=2
  const cs_real_t  d22 = ldlt[5] - l20*d00*l20 - l21*d11*l21;  // m22
  assert(fabs(d22) > cs_math_zero_threshold);
  const cs_real_t  f22 = 1. / d22;
  const cs_real_t  l32 = (ldlt[8] - l30*d00*l20 - l31*d11*l21) * f22;  // m23
 
  // j=3
  const cs_real_t  d33 = ldlt[9] - l30*d00*l30 - l31*d11*l31 - l32*d22*l32; // m33
  assert(fabs(d33) > cs_math_zero_threshold);
  const cs_real_t  f33 = 1. / d33;
 
  ldlt[0] = f00;
  ldlt[1] = l10;
  ldlt[2] = f11;
  ldlt[3] = l20;
  ldlt[4] = l21;
  ldlt[5] = f22;
  ldlt[6] = l30;
  ldlt[7] = l31;
  ldlt[8] = l32;
  ldlt[9] = f33;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE inline cs_real_t
cs_math_sym_44_partial_solve_ldlt(const cs_real_t  ldlt[10],
                                  const cs_real_t  rhs[4])
{
  /* f00, f11, f22, f33, l32, l31, l30, l21, l20, l10
     0    1    2    3    4    5    6    7    8    9   */
 
  cs_real_t x[4]; /* solution */
 
  x[0] = rhs[0];
  x[1] = rhs[1] - x[0]*ldlt[1];
  x[2] = rhs[2] - x[0]*ldlt[3] - x[1]*ldlt[4];
  x[3] = rhs[3] - x[0]*ldlt[6] - x[1]*ldlt[7] - x[2]*ldlt[8];
 
  x[3] = x[3]*ldlt[9];
 
  return x[3];
 
  /*
    x[2] = x[2]*ldlt[5] - ldlt[8]*x[3];
    x[1] = x[1]*ldlt[2] - ldlt[7]*x[3] - ldlt[4]*x[2];
    x[0] = x[0]*ldlt[0] - ldlt[6]*x[3] - ldlt[3]*x[2] - ldlt[1]*x[1];
  */
}
 
/*=============================================================================
 * Public function prototypes
 *============================================================================*/
 
/*----------------------------------------------------------------------------*/
/*
 * \brief  Compute the length (Euclidean norm) between two points xa and xb in
 *         a Cartesian coordinate system of dimension 3
 *
 * \param[in]   xa       coordinate of the first extremity
 * \param[in]   xb       coordinate of the second extremity
 * \param[out]  len      pointer to the length of the vector va -> vb
 * \param[out]  unitv    unitary vector along xa -> xb
 */
/*----------------------------------------------------------------------------*/
 
CS_F_HOST_DEVICE void
cs_math_3_length_unitv(const cs_real_t    xa[3],
                       const cs_real_t    xb[3],
                       cs_real_t         *len,
                       cs_real_3_t        unitv);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
void
cs_math_sym_33_eigen(const cs_real_t  m[6],
                     cs_real_t        eig_vals[3]);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
void
cs_math_33_eigen(const cs_real_t     m[3][3],
                 cs_real_t          *eig_ratio,
                 cs_real_t          *eig_max);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
double
cs_math_surftri(const cs_real_t  xv[3],
                const cs_real_t  xe[3],
                const cs_real_t  xf[3]);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
double
cs_math_voltet(const cs_real_t   xv[3],
               const cs_real_t   xe[3],
               const cs_real_t   xf[3],
               const cs_real_t   xc[3]);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
void
cs_math_33_eig_val_vec(const cs_real_t  m_in[3][3],
                       const cs_real_t  tol_err,
                       cs_real_t        eig_val[3],
                       cs_real_t        eig_vec[3][3]);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
void
cs_math_fact_lu(const int         n,
                const cs_real_t  *a,
                cs_real_t        *a_lu);
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
void
cs_math_fw_and_bw_lu(const cs_real_t  a_lu[],
                     const int        n,
                     cs_real_t        x[],
                     const cs_real_t  b[]);
 
/*----------------------------------------------------------------------------*/
 
END_C_DECLS
 
#endif /* CS_MATH_H */