macros.h

/*
   The MIT License (MIT)

   Copyright (c) 2015 Tomasz Koziara

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in all
   copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   SOFTWARE.
 */

#ifndef ISPC
#include <math.h>
#endif

#ifndef __macros__
#define __macros__

#if defined REAL /* prevent warnings */
#undef REAL
#endif

#if defined REAL_SIZE
#undef REAL_SIZE
#endif

#if defined MPI_REAL
#undef MPI_REAL
#endif

#if defined REAL_MAX
#undef REAL_MAX
#endif

#if defined REAL_EPS
#undef REAL_EPS
#endif

/* real type */
#if REAL==4
#define REAL float
#define REAL_SIZE 4
#define MPI_REAL MPI_FLOAT
#define REAL_MAX 3.40282347E+38F
#define REAL_EPS 1E-4
#else
#define REAL double
#define REAL_SIZE 8
#define MPI_REAL MPI_DOUBLE
#define REAL_MAX 1.7976931348623157E+308
#define REAL_EPS 1E-8
#endif

/* textual assertion */
#define ASSERT(__test__, ...)\
  do {\
    if (!(__test__)) { fprintf (stderr, "%s: %d => ", __FILE__, __LINE__);\
      fprintf (stderr, __VA_ARGS__);\
      fprintf (stderr, "\n"); exit (1); } } while (0)

/* memory validity assertion */
#define ERRMEM(__pointer__) ASSERT (__pointer__, "Out of memory!");

#define DRAND() ((REAL) rand () / (REAL) RAND_MAX)
#define DRANDEXT(x, y) ((x) + ((y) - (x))*DRAND())

#define ALG_PI 3.14159265358979323846 
#define ALG_SQR2 1.4142135623730951

#ifndef MIN
#define MIN(v, w) ((v) < (w) ? (v) : (w))
#endif

#ifndef MAX
#define MAX(v, w) ((v) > (w) ? (v) : (w))
#endif

#define SGN(v) ((v) > 0 ? 1 : ((v) < 0 ? -1 : 0))
#define ABS(v) ((v) > 0 ? (v) : -(v))

/* various algebraic macros for small
 * vectors and matrices follow below */

#define VECTOR(a, b, c, d)\
{\
  (a) [0] = b;\
  (a) [1] = c;\
  (a) [2] = d;\
}

#define ADD(a, b, c)\
{\
  (c) [0] = (a) [0] + (b) [0];\
  (c) [1] = (a) [1] + (b) [1];\
  (c) [2] = (a) [2] + (b) [2];\
}

#define ADD4(a, b, c)\
{\
  (c) [0] = (a) [0] + (b) [0];\
  (c) [1] = (a) [1] + (b) [1];\
  (c) [2] = (a) [2] + (b) [2];\
  (c) [3] = (a) [3] + (b) [3];\
}

#define ADDMUL(a, eps, b, c)\
{\
  (c) [0] = (a) [0] + (eps)*(b) [0];\
  (c) [1] = (a) [1] + (eps)*(b) [1];\
  (c) [2] = (a) [2] + (eps)*(b) [2];\
}

#define SUB(a, b, c)\
{\
  (c) [0] = (a) [0] - (b) [0];\
  (c) [1] = (a) [1] - (b) [1];\
  (c) [2] = (a) [2] - (b) [2];\
}

#define SUB2(a, b, c)\
{\
  (c) [0] = (a) [0] - (b) [0];\
  (c) [1] = (a) [1] - (b) [1];\
}

#define SUB4(a, b, c)\
{\
  (c) [0] = (a) [0] - (b) [0];\
  (c) [1] = (a) [1] - (b) [1];\
  (c) [2] = (a) [2] - (b) [2];\
  (c) [3] = (a) [3] - (b) [3];\
}

#define SUBMUL(a, eps, b, c)\
{\
  (c) [0] = (a) [0] - (eps)*(b) [0];\
  (c) [1] = (a) [1] - (eps)*(b) [1];\
  (c) [2] = (a) [2] - (eps)*(b) [2];\
}

#define MUL(a, eps, b)\
{\
  (b) [0] = (a) [0] * (eps);\
  (b) [1] = (a) [1] * (eps);\
  (b) [2] = (a) [2] * (eps);\
}

#define AXPY(a, eps, b)\
{\
  (b) [0] += (a) [0] * (eps);\
  (b) [1] += (a) [1] * (eps);\
  (b) [2] += (a) [2] * (eps);\
}

#define MUL2(a, eps, b)\
{\
  (b) [0] = (a) [0] * (eps);\
  (b) [1] = (a) [1] * (eps);\
}

#define DIV(a, eps, b)\
{\
  (b) [0] = (a) [0] / (eps);\
  (b) [1] = (a) [1] / (eps);\
  (b) [2] = (a) [2] / (eps);\
}

#define DIV2(a, eps, b)\
{\
  (b) [0] = (a) [0] / (eps);\
  (b) [1] = (a) [1] / (eps);\
}

#define HADAMARD(a, b, c)\
{\
  (c) [0] = (a) [0] * (b) [0];\
  (c) [1] = (a) [1] * (b) [1];\
  (c) [2] = (a) [2] * (b) [2];\
}

#define INVHADAMARD(a, b, c)\
{\
  (c) [0] = (a) [0] / (b) [0];\
  (c) [1] = (a) [1] / (b) [1];\
  (c) [2] = (a) [2] / (b) [2];\
}

#define DOT(a, b)\
  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])

#define DOT2(a, b)\
  ((a)[0]*(b)[0] + (a)[1]*(b)[1])

#define DOT4(a, b) (DOT(a,b) + (a)[3]*(b)[3])

#define DOT5(a, b) (DOT4(a,b) + (a)[4]*(b)[4])

#define LEN(a) (sqrt (DOT (a, a)))

#define LEN2(a) (sqrt (DOT2 (a, a)))

#define LEN4(a) (sqrt (DOT4 (a, a)))

#define PRODUCT(a, b, c)\
{\
  (c) [0] = (a) [1]*(b) [2] - (a) [2]*(b) [1];\
  (c) [1] = (a) [2]*(b) [0] - (a) [0]*(b) [2];\
  (c) [2] = (a) [0]*(b) [1] - (a) [1]*(b) [0];\
}

#define PRODUCTADD(a, b, c)\
{\
  (c) [0] += (a) [1]*(b) [2] - (a) [2]*(b) [1];\
  (c) [1] += (a) [2]*(b) [0] - (a) [0]*(b) [2];\
  (c) [2] += (a) [0]*(b) [1] - (a) [1]*(b) [0];\
}

#define PRODUCTSUB(a, b, c)\
{\
  (c) [0] -= (a) [1]*(b) [2] - (a) [2]*(b) [1];\
  (c) [1] -= (a) [2]*(b) [0] - (a) [0]*(b) [2];\
  (c) [2] -= (a) [0]*(b) [1] - (a) [1]*(b) [0];\
}

#define REVERSE(a, b)\
{\
  (b) [0] = -(a) [0];\
  (b) [1] = -(a) [1];\
  (b) [2] = -(a) [2];\
}

#define SCALE(a, b)\
{\
  (a) [0] *= b;\
  (a) [1] *= b;\
  (a) [2] *= b;\
}

#define SCALE2(a, b)\
{\
  (a) [0] *= b;\
  (a) [1] *= b;\
}

#define SCALE4(a, b)\
{\
  (a) [0] *= b;\
  (a) [1] *= b;\
  (a) [2] *= b;\
  (a) [3] *= b;\
}

#define COPY(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
  (b) [2] = (a) [2];\
}

#define COPY2(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
}

#define COPY4(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
  (b) [2] = (a) [2];\
  (b) [3] = (a) [3];\
}

#define COPY6(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
  (b) [2] = (a) [2];\
  (b) [3] = (a) [3];\
  (b) [4] = (a) [4];\
  (b) [5] = (a) [5];\
}

#define ACC(a, b)\
{\
  (b) [0] += (a) [0];\
  (b) [1] += (a) [1];\
  (b) [2] += (a) [2];\
}

#define SCC(a, b)\
{\
  (b) [0] -= (a) [0];\
  (b) [1] -= (a) [1];\
  (b) [2] -= (a) [2];\
}

#define ACCABS(a, b)\
{\
  (b) [0] += fabs ((a) [0]);\
  (b) [1] += fabs ((a) [1]);\
  (b) [2] += fabs ((a) [2]);\
}

#define ACC6(a, b)\
{\
  (b) [0] += (a) [0];\
  (b) [1] += (a) [1];\
  (b) [2] += (a) [2];\
  (b) [3] += (a) [3];\
  (b) [4] += (a) [4];\
  (b) [5] += (a) [5];\
}

#define COPY12(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
  (b) [2] = (a) [2];\
  (b) [3] = (a) [3];\
  (b) [4] = (a) [4];\
  (b) [5] = (a) [5];\
  (b) [6] = (a) [6];\
  (b) [7] = (a) [7];\
  (b) [8] = (a) [8];\
  (b) [9] = (a) [9];\
  (b) [10] = (a) [10];\
  (b) [11] = (a) [11];\
}

#define ACC12(a, b)\
{\
  (b) [0] += (a) [0];\
  (b) [1] += (a) [1];\
  (b) [2] += (a) [2];\
  (b) [3] += (a) [3];\
  (b) [4] += (a) [4];\
  (b) [5] += (a) [5];\
  (b) [6] += (a) [6];\
  (b) [7] += (a) [7];\
  (b) [8] += (a) [8];\
  (b) [9] += (a) [9];\
  (b) [10] += (a) [10];\
  (b) [11] += (a) [11];\
}

#define SET(a, b)\
{\
  (a) [0] = b;\
  (a) [1] = b;\
  (a) [2] = b;\
}

#define ZERO(a) SET(a, 0.0)

#define SETN(a, n, b) for (REAL *x = (a), *y = x + (n); x != y; x ++) (*x) = (b)

#define ZERON(a, n) SETN(a, n, 0.0)

#define SETRAND(a, b)\
{\
  (a) [0] = DRANDEXT(-b, b);\
  (a) [1] = DRANDEXT(-b, b);\
  (a) [2] = DRANDEXT(-b, b);\
}

#define SET2(a, b)\
{\
  (a) [0] = b;\
  (a) [1] = b;\
}

#define MID(a, b, c)\
{\
  (c) [0] = .5*((a) [0] + (b) [0]);\
  (c) [1] = .5*((a) [1] + (b) [1]);\
  (c) [2] = .5*((a) [2] + (b) [2]);\
}

#define MID3(a, b, c, d)\
{\
  (d) [0] = .333333333333333*((a) [0] + (b) [0] + (c) [0]);\
  (d) [1] = .333333333333333*((a) [1] + (b) [1] + (c) [1]);\
  (d) [2] = .333333333333333*((a) [2] + (b) [2] + (c) [2]);\
}

#define DAMP(beta, a, b, c)\
{\
  (c) [0] = (beta)*(a) [0] + (1.0 - (beta))*(b) [0];\
  (c) [1] = (beta)*(a) [1] + (1.0 - (beta))*(b) [1];\
  (c) [2] = (beta)*(a) [2] + (1.0 - (beta))*(b) [2];\
}

/* triangle (a, b, c) area
 * reads |((a-b)x(c-b))|/2 */
#define TRIANGLE_AREA(a, b, c, area)\
{\
  REAL ab [3], cb [3], nl [3];\
  SUB (a, b, ab);\
  SUB (c, b, cb);\
  PRODUCT (ab, cb, nl);\
  (area) = 0.5 * LEN (nl);\
}

/* counter clock wise (a, b, c) normal */
#define NORMAL(a, b, c, normal)\
{\
  REAL ba [3], cb [3];\
  SUB (b, a, ba);\
  SUB (c, b, cb);\
  PRODUCT (ba, cb, normal);\
}

#define NORMALIZE(a)\
{\
  REAL len = LEN (a);\
  (a) [0] /= len;\
  (a) [1] /= len;\
  (a) [2] /= len;\
}

#define PLANE(pln, pnt) (DOT (pln, pnt) + (pln) [3])

/* p = 4-plane and line segment (a, b) intersection; call only for intersecting entities */
#define PLANESEG(plane, a, b, p)\
{\
  REAL l [3], s;\
  \
  l [0] = b [0] - a [0];\
  l [1] = b [1] - a [1];\
  l [2] = b [2] - a [2];\
  s = PLANE (plane, a) / DOT (plane, l);\
  p [0] = a [0] - l [0] * s;\
  p [1] = a [1] - l [1] * s;\
  p [2] = a [2] - l [2] * s;\
}

#define PLANECOPY(a, b)\
{\
  (b) [0] = (a) [0];\
  (b) [1] = (a) [1];\
  (b) [2] = (a) [2];\
  (b) [3] = (a) [3];\
}

#define MAXABS(a, maximum)\
{\
  REAL a0 = ABS((a)[0]),\
  a1 = ABS((a)[1]),\
  a2 = ABS((a)[2]);\
  (maximum) = a0;\
  if ((maximum) < a1) (maximum) = a1;\
  if ((maximum) < a2) (maximum) = a2;\
}

#define MAXABSIDX(a, imax)\
{\
  REAL a0 = ABS((a)[0]),\
  a1 = ABS((a)[1]),\
  a2 = ABS((a)[2]),\
  maximum = a0;\
  (imax) = 0;\
  if (maximum < a1) {maximum = a1; (imax) = 1;}\
  if (maximum < a2) {maximum = a2; (imax) = 2;}\
}

#define MAXABS2(a, maximum)\
{\
  REAL a0 = ABS((a)[0]),\
  a1 = ABS((a)[1]);\
  (maximum) = a0;\
  if ((maximum) < a1) (maximum) = a1;\
}

#define MAXABSN(a, n, maximum)\
{\
  (maximum) = ABS((a)[0]);\
  for (int i = 1; i < (n); i ++)\
  {\
    REAL ai = ABS((a)[i]);\
    if ((maximum) < ai) (maximum) = ai;\
  }\
}

#define FILTERN(a, n, tol)\
{\
  for (int i = 0; i < (n); i ++)\
  {\
    REAL ai = ABS((a)[i]);\
    if (ai <= tol) (a)[i] = 0;\
  }\
}

#define NORMALIZE4(a)\
{\
  REAL len = LEN4 (a);\
  (a) [0] /= len;\
  (a) [1] /= len;\
  (a) [2] /= len;\
  (a) [3] /= len;\
}

#define SCALEDIAG(A, EPS)\
{\
  (A) [0] *= (EPS);\
  (A) [4] *= (EPS);\
  (A) [8] *= (EPS);\
}

#define SET6(A, EPS)\
{\
  (A) [0] = (EPS);\
  (A) [1] = (EPS);\
  (A) [2] = (EPS);\
  (A) [3] = (EPS);\
  (A) [4] = (EPS);\
  (A) [5] = (EPS);\
}

#define SET9(A, EPS)\
{\
  (A) [0] = (EPS);\
  (A) [1] = (EPS);\
  (A) [2] = (EPS);\
  (A) [3] = (EPS);\
  (A) [4] = (EPS);\
  (A) [5] = (EPS);\
  (A) [6] = (EPS);\
  (A) [7] = (EPS);\
  (A) [8] = (EPS);\
}

#define SET12(A, EPS)\
{\
  (A) [0] = (EPS);\
  (A) [1] = (EPS);\
  (A) [2] = (EPS);\
  (A) [3] = (EPS);\
  (A) [4] = (EPS);\
  (A) [5] = (EPS);\
  (A) [6] = (EPS);\
  (A) [7] = (EPS);\
  (A) [8] = (EPS);\
  (A) [9] = (EPS);\
  (A) [10] = (EPS);\
  (A) [11] = (EPS);\
}

#define SCALE6(A, EPS)\
{\
  (A) [0] *= (EPS);\
  (A) [1] *= (EPS);\
  (A) [2] *= (EPS);\
  (A) [3] *= (EPS);\
  (A) [4] *= (EPS);\
  (A) [5] *= (EPS);\
}

#define SCALE9(A, EPS)\
{\
  (A) [0] *= (EPS);\
  (A) [1] *= (EPS);\
  (A) [2] *= (EPS);\
  (A) [3] *= (EPS);\
  (A) [4] *= (EPS);\
  (A) [5] *= (EPS);\
  (A) [6] *= (EPS);\
  (A) [7] *= (EPS);\
  (A) [8] *= (EPS);\
}

#define SCALE12(A, EPS)\
{\
  (A) [0] *= (EPS);\
  (A) [1] *= (EPS);\
  (A) [2] *= (EPS);\
  (A) [3] *= (EPS);\
  (A) [4] *= (EPS);\
  (A) [5] *= (EPS);\
  (A) [6] *= (EPS);\
  (A) [7] *= (EPS);\
  (A) [8] *= (EPS);\
  (A) [9] *= (EPS);\
  (A) [10] *= (EPS);\
  (A) [11] *= (EPS);\
}

#define DOT6(A, B)\
  ((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]+\
   (A)[3]*(B)[3]+(A)[4]*(B)[4]+(A)[5]*(B)[5])\

#define LEN6(A) (sqrt (DOT6 (A, A)))

#define DOT9(A, B)\
  ((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]+\
   (A)[3]*(B)[3]+(A)[4]*(B)[4]+(A)[5]*(B)[5]+\
   (A)[6]*(B)[6]+(A)[7]*(B)[7]+(A)[8]*(B)[8])\

#define LEN9(A) (sqrt (DOT9 (A, A)))

#define DOT12(A, B)\
  ((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]+\
   (A)[3]*(B)[3]+(A)[4]*(B)[4]+(A)[5]*(B)[5]+\
   (A)[6]*(B)[6]+(A)[7]*(B)[7]+(A)[8]*(B)[8]+\
   (A)[9]*(B)[9]+(A)[10]*(B)[10]+(A)[11]*(B)[11])\

#define LEN12(A) (sqrt (DOT12 (A, A)))

#define ADD6(A, B, C)\
{\
  (C) [0] = (A)[0] + (B)[0];\
  (C) [1] = (A)[1] + (B)[1];\
  (C) [2] = (A)[2] + (B)[2];\
  (C) [3] = (A)[3] + (B)[3];\
  (C) [4] = (A)[4] + (B)[4];\
  (C) [5] = (A)[5] + (B)[5];\
}

#define SUB6(A, B, C)\
{\
  (C) [0] = (A)[0] - (B)[0];\
  (C) [1] = (A)[1] - (B)[1];\
  (C) [2] = (A)[2] - (B)[2];\
  (C) [3] = (A)[3] - (B)[3];\
  (C) [4] = (A)[4] - (B)[4];\
  (C) [5] = (A)[5] - (B)[5];\
}

#define ADD12(A, B, C)\
{\
  (C) [0] = (A)[0] + (B)[0];\
  (C) [1] = (A)[1] + (B)[1];\
  (C) [2] = (A)[2] + (B)[2];\
  (C) [3] = (A)[3] + (B)[3];\
  (C) [4] = (A)[4] + (B)[4];\
  (C) [5] = (A)[5] + (B)[5];\
  (C) [6] = (A)[6] + (B)[6];\
  (C) [7] = (A)[7] + (B)[7];\
  (C) [8] = (A)[8] + (B)[8];\
  (C) [9] = (A)[9] + (B)[9];\
  (C) [10] = (A)[10] + (B)[10];\
  (C) [11] = (A)[11] + (B)[11];\
}

#define SUB12(A, B, C)\
{\
  (C) [0] = (A)[0] - (B)[0];\
  (C) [1] = (A)[1] - (B)[1];\
  (C) [2] = (A)[2] - (B)[2];\
  (C) [3] = (A)[3] - (B)[3];\
  (C) [4] = (A)[4] - (B)[4];\
  (C) [5] = (A)[5] - (B)[5];\
  (C) [6] = (A)[6] - (B)[6];\
  (C) [7] = (A)[7] - (B)[7];\
  (C) [8] = (A)[8] - (B)[8];\
  (C) [9] = (A)[9] - (B)[9];\
  (C) [10] = (A)[10] - (B)[10];\
  (C) [11] = (A)[11] - (B)[11];\
}

#define MAX9(A, NORM)\
{\
  REAL __AUX__;\
  (NORM) = 0.0;\
  __AUX__ = fabs ((A) [0]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [1]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [2]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [3]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [4]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [5]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [6]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [7]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
  __AUX__ = fabs ((A) [8]);\
  if (__AUX__ > (NORM)) (NORM) = __AUX__;\
}

#define HADAMARD9(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0];\
  (C) [1] = (A)[1]*(B)[1];\
  (C) [2] = (A)[2]*(B)[2];\
  (C) [3] = (A)[3]*(B)[3];\
  (C) [4] = (A)[4]*(B)[4];\
  (C) [5] = (A)[5]*(B)[5];\
  (C) [6] = (A)[6]*(B)[6];\
  (C) [7] = (A)[7]*(B)[7];\
  (C) [8] = (A)[8]*(B)[8];\
}

#define NNCOPY(A, B)\
{\
  (B) [0] = (A) [0];\
  (B) [1] = (A) [1];\
  (B) [2] = (A) [2];\
  (B) [3] = (A) [3];\
  (B) [4] = (A) [4];\
  (B) [5] = (A) [5];\
  (B) [6] = (A) [6];\
  (B) [7] = (A) [7];\
  (B) [8] = (A) [8];\
}

#define TNCOPY(A, B)\
{\
  (B) [0] = (A) [0];\
  (B) [1] = (A) [3];\
  (B) [2] = (A) [6];\
  (B) [3] = (A) [1];\
  (B) [4] = (A) [4];\
  (B) [5] = (A) [7];\
  (B) [6] = (A) [2];\
  (B) [7] = (A) [5];\
  (B) [8] = (A) [8];\
}

#define NTCOPY(A, B)\
{\
  (B) [0] = (A) [0];\
  (B) [3] = (A) [1];\
  (B) [6] = (A) [2];\
  (B) [1] = (A) [3];\
  (B) [4] = (A) [4];\
  (B) [7] = (A) [5];\
  (B) [2] = (A) [6];\
  (B) [5] = (A) [7];\
  (B) [8] = (A) [8];\
}

#define NNSUB(A, B, C)\
{\
  (C) [0] = (A)[0] - (B)[0];\
  (C) [1] = (A)[1] - (B)[1];\
  (C) [2] = (A)[2] - (B)[2];\
  (C) [3] = (A)[3] - (B)[3];\
  (C) [4] = (A)[4] - (B)[4];\
  (C) [5] = (A)[5] - (B)[5];\
  (C) [6] = (A)[6] - (B)[6];\
  (C) [7] = (A)[7] - (B)[7];\
  (C) [8] = (A)[8] - (B)[8];\
}

#define TNSUB(A, B, C)\
{\
  (C) [0] = (A)[0] - (B)[0];\
  (C) [1] = (A)[3] - (B)[1];\
  (C) [2] = (A)[6] - (B)[2];\
  (C) [3] = (A)[1] - (B)[3];\
  (C) [4] = (A)[4] - (B)[4];\
  (C) [5] = (A)[7] - (B)[5];\
  (C) [6] = (A)[2] - (B)[6];\
  (C) [7] = (A)[5] - (B)[7];\
  (C) [8] = (A)[8] - (B)[8];\
}

#define NTSUB(A, B, C)\
{\
  (C) [0] = (A)[0] - (B)[0];\
  (C) [1] = (A)[1] - (B)[3];\
  (C) [2] = (A)[2] - (B)[6];\
  (C) [3] = (A)[3] - (B)[1];\
  (C) [4] = (A)[4] - (B)[4];\
  (C) [5] = (A)[5] - (B)[7];\
  (C) [6] = (A)[6] - (B)[2];\
  (C) [7] = (A)[7] - (B)[5];\
  (C) [8] = (A)[8] - (B)[8];\
}

#define NNADD(A, B, C)\
{\
  (C) [0] = (A)[0] + (B)[0];\
  (C) [1] = (A)[1] + (B)[1];\
  (C) [2] = (A)[2] + (B)[2];\
  (C) [3] = (A)[3] + (B)[3];\
  (C) [4] = (A)[4] + (B)[4];\
  (C) [5] = (A)[5] + (B)[5];\
  (C) [6] = (A)[6] + (B)[6];\
  (C) [7] = (A)[7] + (B)[7];\
  (C) [8] = (A)[8] + (B)[8];\
}

#define TNADD(A, B, C)\
{\
  (C) [0] = (A)[0] + (B)[0];\
  (C) [1] = (A)[3] + (B)[1];\
  (C) [2] = (A)[6] + (B)[2];\
  (C) [3] = (A)[1] + (B)[3];\
  (C) [4] = (A)[4] + (B)[4];\
  (C) [5] = (A)[7] + (B)[5];\
  (C) [6] = (A)[2] + (B)[6];\
  (C) [7] = (A)[5] + (B)[7];\
  (C) [8] = (A)[8] + (B)[8];\
}

#define NTADD(A, B, C)\
{\
  (C) [0] = (A)[0] + (B)[0];\
  (C) [1] = (A)[1] + (B)[3];\
  (C) [2] = (A)[2] + (B)[6];\
  (C) [3] = (A)[3] + (B)[1];\
  (C) [4] = (A)[4] + (B)[4];\
  (C) [5] = (A)[5] + (B)[7];\
  (C) [6] = (A)[6] + (B)[2];\
  (C) [7] = (A)[7] + (B)[5];\
  (C) [8] = (A)[8] + (B)[8];\
}

#define NNSUBMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] - (EPS)*(B)[0];\
  (C) [1] = (A)[1] - (EPS)*(B)[1];\
  (C) [2] = (A)[2] - (EPS)*(B)[2];\
  (C) [3] = (A)[3] - (EPS)*(B)[3];\
  (C) [4] = (A)[4] - (EPS)*(B)[4];\
  (C) [5] = (A)[5] - (EPS)*(B)[5];\
  (C) [6] = (A)[6] - (EPS)*(B)[6];\
  (C) [7] = (A)[7] - (EPS)*(B)[7];\
  (C) [8] = (A)[8] - (EPS)*(B)[8];\
}

#define TNSUBMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] - (EPS)*(B)[0];\
  (C) [1] = (A)[3] - (EPS)*(B)[1];\
  (C) [2] = (A)[6] - (EPS)*(B)[2];\
  (C) [3] = (A)[1] - (EPS)*(B)[3];\
  (C) [4] = (A)[4] - (EPS)*(B)[4];\
  (C) [5] = (A)[7] - (EPS)*(B)[5];\
  (C) [6] = (A)[2] - (EPS)*(B)[6];\
  (C) [7] = (A)[5] - (EPS)*(B)[7];\
  (C) [8] = (A)[8] - (EPS)*(B)[8];\
}

#define NTSUBMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] - (EPS)*(B)[0];\
  (C) [1] = (A)[1] - (EPS)*(B)[3];\
  (C) [2] = (A)[2] - (EPS)*(B)[6];\
  (C) [3] = (A)[3] - (EPS)*(B)[1];\
  (C) [4] = (A)[4] - (EPS)*(B)[4];\
  (C) [5] = (A)[5] - (EPS)*(B)[7];\
  (C) [6] = (A)[6] - (EPS)*(B)[2];\
  (C) [7] = (A)[7] - (EPS)*(B)[5];\
  (C) [8] = (A)[8] - (EPS)*(B)[8];\
}

#define NNADDMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] + (EPS)*(B)[0];\
  (C) [1] = (A)[1] + (EPS)*(B)[1];\
  (C) [2] = (A)[2] + (EPS)*(B)[2];\
  (C) [3] = (A)[3] + (EPS)*(B)[3];\
  (C) [4] = (A)[4] + (EPS)*(B)[4];\
  (C) [5] = (A)[5] + (EPS)*(B)[5];\
  (C) [6] = (A)[6] + (EPS)*(B)[6];\
  (C) [7] = (A)[7] + (EPS)*(B)[7];\
  (C) [8] = (A)[8] + (EPS)*(B)[8];\
}

#define TNADDMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] + (EPS)*(B)[0];\
  (C) [1] = (A)[3] + (EPS)*(B)[1];\
  (C) [2] = (A)[6] + (EPS)*(B)[2];\
  (C) [3] = (A)[1] + (EPS)*(B)[3];\
  (C) [4] = (A)[4] + (EPS)*(B)[4];\
  (C) [5] = (A)[7] + (EPS)*(B)[5];\
  (C) [6] = (A)[2] + (EPS)*(B)[6];\
  (C) [7] = (A)[5] + (EPS)*(B)[7];\
  (C) [8] = (A)[8] + (EPS)*(B)[8];\
}

#define NTADDMUL(A, EPS, B, C)\
{\
  (C) [0] = (A)[0] + (EPS)*(B)[0];\
  (C) [1] = (A)[1] + (EPS)*(B)[3];\
  (C) [2] = (A)[2] + (EPS)*(B)[6];\
  (C) [3] = (A)[3] + (EPS)*(B)[1];\
  (C) [4] = (A)[4] + (EPS)*(B)[4];\
  (C) [5] = (A)[5] + (EPS)*(B)[7];\
  (C) [6] = (A)[6] + (EPS)*(B)[2];\
  (C) [7] = (A)[7] + (EPS)*(B)[5];\
  (C) [8] = (A)[8] + (EPS)*(B)[8];\
}

#define NNMUL(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[3]*(B)[1]+(A)[6]*(B)[2];\
  (C) [1] = (A)[1]*(B)[0]+(A)[4]*(B)[1]+(A)[7]*(B)[2];\
  (C) [2] = (A)[2]*(B)[0]+(A)[5]*(B)[1]+(A)[8]*(B)[2];\
  (C) [3] = (A)[0]*(B)[3]+(A)[3]*(B)[4]+(A)[6]*(B)[5];\
  (C) [4] = (A)[1]*(B)[3]+(A)[4]*(B)[4]+(A)[7]*(B)[5];\
  (C) [5] = (A)[2]*(B)[3]+(A)[5]*(B)[4]+(A)[8]*(B)[5];\
  (C) [6] = (A)[0]*(B)[6]+(A)[3]*(B)[7]+(A)[6]*(B)[8];\
  (C) [7] = (A)[1]*(B)[6]+(A)[4]*(B)[7]+(A)[7]*(B)[8];\
  (C) [8] = (A)[2]*(B)[6]+(A)[5]*(B)[7]+(A)[8]*(B)[8];\
}

#define TNMUL(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2];\
  (C) [1] = (A)[3]*(B)[0]+(A)[4]*(B)[1]+(A)[5]*(B)[2];\
  (C) [2] = (A)[6]*(B)[0]+(A)[7]*(B)[1]+(A)[8]*(B)[2];\
  (C) [3] = (A)[0]*(B)[3]+(A)[1]*(B)[4]+(A)[2]*(B)[5];\
  (C) [4] = (A)[3]*(B)[3]+(A)[4]*(B)[4]+(A)[5]*(B)[5];\
  (C) [5] = (A)[6]*(B)[3]+(A)[7]*(B)[4]+(A)[8]*(B)[5];\
  (C) [6] = (A)[0]*(B)[6]+(A)[1]*(B)[7]+(A)[2]*(B)[8];\
  (C) [7] = (A)[3]*(B)[6]+(A)[4]*(B)[7]+(A)[5]*(B)[8];\
  (C) [8] = (A)[6]*(B)[6]+(A)[7]*(B)[7]+(A)[8]*(B)[8];\
}

#define NTMUL(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[3]*(B)[3]+(A)[6]*(B)[6];\
  (C) [1] = (A)[1]*(B)[0]+(A)[4]*(B)[3]+(A)[7]*(B)[6];\
  (C) [2] = (A)[2]*(B)[0]+(A)[5]*(B)[3]+(A)[8]*(B)[6];\
  (C) [3] = (A)[0]*(B)[1]+(A)[3]*(B)[4]+(A)[6]*(B)[7];\
  (C) [4] = (A)[1]*(B)[1]+(A)[4]*(B)[4]+(A)[7]*(B)[7];\
  (C) [5] = (A)[2]*(B)[1]+(A)[5]*(B)[4]+(A)[8]*(B)[7];\
  (C) [6] = (A)[0]*(B)[2]+(A)[3]*(B)[5]+(A)[6]*(B)[8];\
  (C) [7] = (A)[1]*(B)[2]+(A)[4]*(B)[5]+(A)[7]*(B)[8];\
  (C) [8] = (A)[2]*(B)[2]+(A)[5]*(B)[5]+(A)[8]*(B)[8];\
}

#define NVMUL(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[3]*(B)[1]+(A)[6]*(B)[2];\
  (C) [1] = (A)[1]*(B)[0]+(A)[4]*(B)[1]+(A)[7]*(B)[2];\
  (C) [2] = (A)[2]*(B)[0]+(A)[5]*(B)[1]+(A)[8]*(B)[2];\
}

#define NVMUL2(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[2]*(B)[1];\
  (C) [1] = (A)[1]*(B)[0]+(A)[3]*(B)[1];\
}

#define NVMUL4(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[4]*(B)[1]+(A)[8]*(B)[2]+(A)[12]*(B)[3];\
  (C) [1] = (A)[1]*(B)[0]+(A)[5]*(B)[1]+(A)[9]*(B)[2]+(A)[13]*(B)[3];\
  (C) [2] = (A)[2]*(B)[0]+(A)[6]*(B)[1]+(A)[10]*(B)[2]+(A)[14]*(B)[3];\
  (C) [3] = (A)[3]*(B)[0]+(A)[7]*(B)[1]+(A)[11]*(B)[2]+(A)[15]*(B)[3];\
}

#define TVMUL(A, B, C)\
{\
  (C) [0] = (A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2];\
  (C) [1] = (A)[3]*(B)[0]+(A)[4]*(B)[1]+(A)[5]*(B)[2];\
  (C) [2] = (A)[6]*(B)[0]+(A)[7]*(B)[1]+(A)[8]*(B)[2];\
}

#define NVADDMUL(C, A, B, D)\
{\
  (D) [0] = (C)[0] + ((A)[0]*(B)[0]+(A)[3]*(B)[1]+(A)[6]*(B)[2]);\
  (D) [1] = (C)[1] + ((A)[1]*(B)[0]+(A)[4]*(B)[1]+(A)[7]*(B)[2]);\
  (D) [2] = (C)[2] + ((A)[2]*(B)[0]+(A)[5]*(B)[1]+(A)[8]*(B)[2]);\
}

#define TVADDMUL(C, A, B, D)\
{\
  (D) [0] = (C)[0] + ((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]);\
  (D) [1] = (C)[1] + ((A)[3]*(B)[0]+(A)[4]*(B)[1]+(A)[5]*(B)[2]);\
  (D) [2] = (C)[2] + ((A)[6]*(B)[0]+(A)[7]*(B)[1]+(A)[8]*(B)[2]);\
}

#define NVSUBMUL(C, A, B, D)\
{\
  (D) [0] = (C)[0] - ((A)[0]*(B)[0]+(A)[3]*(B)[1]+(A)[6]*(B)[2]);\
  (D) [1] = (C)[1] - ((A)[1]*(B)[0]+(A)[4]*(B)[1]+(A)[7]*(B)[2]);\
  (D) [2] = (C)[2] - ((A)[2]*(B)[0]+(A)[5]*(B)[1]+(A)[8]*(B)[2]);\
}

#define TVSUBMUL(C, A, B, D)\
{\
  (D) [0] = (C)[0] - ((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]);\
  (D) [1] = (C)[1] - ((A)[3]*(B)[0]+(A)[4]*(B)[1]+(A)[5]*(B)[2]);\
  (D) [2] = (C)[2] - ((A)[6]*(B)[0]+(A)[7]*(B)[1]+(A)[8]*(B)[2]);\
}

#define TRACE2(A) ((A)[0] + (A)[3])

#define TRACE(A) ((A)[0] + (A)[4] + (A)[8])

#define IDENTITY(A)\
{\
  (A) [0] = 1;\
  (A) [1] = 0;\
  (A) [2] = 0;\
  (A) [3] = 0;\
  (A) [4] = 1;\
  (A) [5] = 0;\
  (A) [6] = 0;\
  (A) [7] = 0;\
  (A) [8] = 1;\
}

#define DET2(F) (((F)[0]*(F)[3]-(F)[1]*(F)[2]))

#define INVERT2(F, INV, DET)\
  if (((DET)=((F)[0]*(F)[3]-(F)[1]*(F)[2])) != 0.0)\
{\
  (INV) [0] =  (F)[3] / (DET);\
  (INV) [1] = -(F)[1] / (DET);\
  (INV) [2] = -(F)[2] / (DET);\
  (INV) [3] =  (F)[0] / (DET);\
}

#define DET(F) ((F)[0]*((F)[4]*(F)[8]-(F)[5]*(F)[7])+\
    (F)[3]*((F)[2]*(F)[7]-(F)[1]*(F)[8])+\
    (F)[6]*((F)[1]*(F)[5]-(F)[2]*(F)[4]))

#define INVERT(F, INV, DET)\
  if (((DET) =\
        ((F)[0]*((F)[4]*(F)[8]-(F)[5]*(F)[7])+\
         (F)[3]*((F)[2]*(F)[7]-(F)[1]*(F)[8])+\
         ((F)[1]*(F)[5]-(F)[2]*(F)[4])*(F)[6])) != 0.0)\
{\
  (DET) = 1.0 / (DET);\
  (INV) [0] = ((F)[4]*(F)[8]-(F)[5]*(F)[7])*(DET);\
  (INV) [1] = ((F)[2]*(F)[7]-(F)[1]*(F)[8])*(DET);\
  (INV) [2] = ((F)[1]*(F)[5]-(F)[2]*(F)[4])*(DET);\
  (INV) [3] = ((F)[5]*(F)[6]-(F)[3]*(F)[8])*(DET);\
  (INV) [4] = ((F)[0]*(F)[8]-(F)[2]*(F)[6])*(DET);\
  (INV) [5] = ((F)[2]*(F)[3]-(F)[0]*(F)[5])*(DET);\
  (INV) [6] = ((F)[3]*(F)[7]-(F)[4]*(F)[6])*(DET);\
  (INV) [7] = ((F)[1]*(F)[6]-(F)[0]*(F)[7])*(DET);\
  (INV) [8] = ((F)[0]*(F)[4]-(F)[1]*(F)[3])*(DET);\
  (DET) = 1.0 / (DET);\
}

#define SYMM(A, SYMM)\
{\
  (SYMM) [0] = (A) [0];\
  (SYMM) [1] = 0.5 * ((A) [1] + (A) [3]);\
  (SYMM) [2] = 0.5 * ((A) [2] + (A) [6]);\
  (SYMM) [3] = (SYMM) [1];\
  (SYMM) [4] = (A) [4];\
  (SYMM) [5] = 0.5 * ((A) [5] + (A) [7]);\
  (SYMM) [6] = (SYMM) [2];\
  (SYMM) [7] = (SYMM) [5];\
  (SYMM) [8] = (A) [8];\
}

#define SKEW(A, SKEW)\
{\
  (SKEW) [0] = 0;\
  (SKEW) [1] = 0.5 * ((A) [1] - (A) [3]);\
  (SKEW) [2] = -0.5 * ((A) [2] - (A) [6]);\
  (SKEW) [3] = -(SKEW) [1];\
  (SKEW) [4] = 0;\
  (SKEW) [5] = 0.5 * ((A) [5] - (A) [7]);\
  (SKEW) [6] = -(SKEW) [2];\
  (SKEW) [7] = -(SKEW) [5];\
  (SKEW) [8] = 0;\
}

#define SKEWVEC(A, VSKEW)\
{\
  (VSKEW) [0] = 0.5 * ((A) [5] - (A) [7]);\
  (VSKEW) [1] = 0.5 * ((A) [6] - (A) [2]);\
  (VSKEW) [2] = 0.5 * ((A) [1] - (A) [3]);\
}

#define VECSKEW(VSKEW, A)\
{\
  (A) [0] = 0;\
  (A) [1] = (VSKEW) [2];\
  (A) [2] = -(VSKEW) [1];\
  (A) [3] = -(VSKEW) [2];\
  (A) [4] = 0;\
  (A) [5] = (VSKEW) [0];\
  (A) [6] = (VSKEW) [1];\
  (A) [7] = -(VSKEW) [0];\
  (A) [8] = 0;\
}

#define SYMMLOTR(SYMM,LO)\
  (LO)[0] = (SYMM)[0], (LO)[1] = (SYMM)[1], (LO)[2] = (SYMM)[2],\
  (LO)[3] = (SYMM)[4], (LO)[4] = (SYMM)[5], (LO)[5] = (SYMM)[8]

#define LOTRSYMM(LO,SYMM)\
  (SYMM)[0] = (LO)[0], (SYMM)[1] = (LO)[1], (SYMM)[2] = (LO)[2],\
  (SYMM)[3] = (LO)[1], (SYMM)[4] = (LO)[3], (SYMM)[5] = (LO)[4],\
  (SYMM)[6] = (LO)[2], (SYMM)[7] = (LO)[4], (SYMM)[8] = (LO)[5]

#define DIADIC(a, b, C)\
{\
  (C) [0] = (a) [0]*(b) [0];\
  (C) [1] = (a) [1]*(b) [0];\
  (C) [2] = (a) [2]*(b) [0];\
  (C) [3] = (a) [0]*(b) [1];\
  (C) [4] = (a) [1]*(b) [1];\
  (C) [5] = (a) [2]*(b) [1];\
  (C) [6] = (a) [0]*(b) [2];\
  (C) [7] = (a) [1]*(b) [2];\
  (C) [8] = (a) [2]*(b) [2];\
}

/* polar decomposition of 'F' with 'EPS' tollerance,
 * outputing 'R' and 'U' computed after 'ITERS' iterations */
#define POLAR(F, EPS, R, U, ITERS)\
{\
  REAL __TMP__ [9],\
  __L2__, __MX__,\
  __IL2__, __IMX__,\
  __GAMMA__, __DET__,\
  __EPS2__ = (EPS)*(EPS);\
  NNCOPY (F, R);\
  (ITERS) = 0;\
  do\
  {\
    (ITERS) ++;\
    NNCOPY (R, U);\
    INVERT (R, __TMP__, __DET__);\
    if (__DET__ == 0.0)\
    { (ITERS) = -1; break; }\
    __L2__ = LEN9 (R);\
    MAX9 (R, __MX__);\
    __IL2__ = LEN9 (__TMP__);\
    MAX9 (__TMP__, __IMX__);\
    __GAMMA__ = sqrt(sqrt((__IL2__*__IMX__)/\
          (__L2__*__MX__))) * 0.5;\
    SCALE9 (R, __GAMMA__);\
    __GAMMA__ = 0.25 / __GAMMA__;\
    SCALE9 (__TMP__, __GAMMA__);\
    NTADD (R, __TMP__, R);\
    NNSUB (R, U, __TMP__);\
  } while (DOT9 (__TMP__, __TMP__) >\
      __EPS2__ * DOT9 (U, U));\
  TNMUL (R, F, __TMP__);\
  SYMM (__TMP__, U);\
}

/* ten degrees squared in radians =>
 * below ten degrees the exponential map
 * below will use Taylor expansions */
#define DEG_10_SQ 3.0461741978671E-02

/* compute an exponential map; given
 * the rotationa vector 'VSKEW' output
 * the orthogonal rotation matrix 'R' */
#define EXPMAP(VSKEW, R)\
{\
  REAL __ANG_2__, __SIN_X__,\
  __1_COS_XX__, __0__, __1__, __2__,\
  __01__, __02__, __12__, __S0__,\
  __S1__, __S2__;\
  __ANG_2__ = DOT(VSKEW, VSKEW);\
  if (__ANG_2__ < DEG_10_SQ)\
  {\
    __SIN_X__ = 1.0 +\
    (-1.666666666666667E-1 +\
     (8.333333333333333E-3 +\
      (-1.984126984126984E-4 +\
       (2.755731922398589E-6 +\
        (-2.505210838544172E-8 +\
         1.605904383682161E-10 * __ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__;\
    __1_COS_XX__ = 0.5 +\
    (-4.166666666666667E-2 +\
     (1.388888888888889E-3 +\
      (-2.480158730158730E-5 +\
       (2.755731922398589E-7 +\
        (-2.087675698786810E-9 +\
         1.147074559772972E-11 * __ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__;\
  }\
  else\
  {\
    __0__ = __ANG_2__;\
    __ANG_2__ = sqrt (__ANG_2__);\
    __SIN_X__ = sin (__ANG_2__) / __ANG_2__;\
    __1_COS_XX__ = (1.0 - cos (__ANG_2__)) / __0__;\
  }\
  __0__ = (VSKEW) [0] * (VSKEW) [0];\
  __1__ = (VSKEW) [1] * (VSKEW) [1];\
  __2__ = (VSKEW) [2] * (VSKEW) [2];\
  __01__ = (VSKEW) [0] * (VSKEW) [1];\
  __02__ = (VSKEW) [0] * (VSKEW) [2];\
  __12__ = (VSKEW) [1] * (VSKEW) [2];\
  __S0__ = __SIN_X__ * (VSKEW) [0];\
  __S1__ = __SIN_X__ * (VSKEW) [1];\
  __S2__ = __SIN_X__ * (VSKEW) [2];\
  (R) [0] = - __1_COS_XX__*(__2__+__1__);\
  (R) [1] = __1_COS_XX__*__01__;\
  (R) [2] = __1_COS_XX__*__02__;\
  (R) [3] = (R) [1];\
  (R) [4] = - __1_COS_XX__*(__2__+__0__);\
  (R) [5] = __1_COS_XX__*__12__;\
  (R) [6] = (R) [2];\
  (R) [7] = (R) [5];\
  (R) [8] = - __1_COS_XX__*(__1__+__0__);\
  (R) [0] += 1.0;\
  (R) [1] += __S2__;\
  (R) [2] -= __S1__;\
  (R) [3] -= __S2__;\
  (R) [4] += 1.0;\
  (R) [5] += __S0__;\
  (R) [6] += __S1__;\
  (R) [7] -= __S0__;\
  (R) [8] += 1.0;\
}

/* compute differential of the exponential map =>
 * this is useful for moving vectors between tangent spaces
 * spanned at identity and 'VECSKEW' */
#define DEXPMAP(VSKEW, R)\
{\
  REAL __ANG_2__, __1_XX_SIN_XXX__,\
  __1_COS_XX__, __0__, __1__, __2__,\
  __S0__, __S1__, __S2__;\
  __ANG_2__ = DOT(VSKEW, VSKEW);\
  if (__ANG_2__ < DEG_10_SQ)\
  {\
    __1_XX_SIN_XXX__ = \
    1.666666666666667E-1 +\
    (-8.333333333333333E-3 +\
     (1.984126984126984E-4 +\
      (-2.755731922398589E-6 +\
       (2.505210838544172E-8 +\
        (-1.605904383682161E-10+\
         7.647163731819816E-13 * __ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__;\
    __1_COS_XX__ = 0.5 +\
    (-4.166666666666667E-2 +\
     (1.388888888888889E-3 +\
      (-2.480158730158730E-5 +\
       (2.755731922398589E-7 +\
        (-2.087675698786810E-9 +\
         1.147074559772972E-11 * __ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__\
         )*__ANG_2__;\
  }\
  else\
  {\
    __0__ = __ANG_2__;\
    __ANG_2__ = sqrt (__ANG_2__);\
    __1_XX_SIN_XXX__ = (1.0 / __0__) - (sin (__ANG_2__) / (__0__*__ANG_2__));\
    __1_COS_XX__ = (1.0 - cos (__ANG_2__)) / __0__;\
  }\
  __0__ = (VSKEW)[0]*(VSKEW)[0];\
  __1__ = (VSKEW)[1]*(VSKEW)[1];\
  __2__ = (VSKEW)[2]*(VSKEW)[2];\
  __S0__ = __1_COS_XX__ * (VSKEW) [0];\
  __S1__ = __1_COS_XX__ * (VSKEW) [1];\
  __S2__ = __1_COS_XX__ * (VSKEW) [2];\
  (R) [0] = __1_XX_SIN_XXX__ * (-__2__-__1__);\
  (R) [1] = __1_XX_SIN_XXX__ * (VSKEW)[0]*(VSKEW)[1];\
  (R) [2] = __1_XX_SIN_XXX__ * (VSKEW)[0]*(VSKEW)[2];\
  (R) [3] = (R) [1];\
  (R) [4] = __1_XX_SIN_XXX__ * (-__2__-__0__); \
  (R) [5] =  __1_XX_SIN_XXX__ * (VSKEW)[1]*(VSKEW)[2];\
  (R) [6] = (R) [2];\
  (R) [7] = (R) [5];\
  (R) [8] = __1_XX_SIN_XXX__ * (-__1__-__0__);\
  (R) [0] += 1.0;\
  (R) [1] += __S2__;\
  (R) [2] -= __S1__;\
  (R) [3] -= __S2__;\
  (R) [4] += 1.0;\
  (R) [5] += __S0__;\
  (R) [6] += __S1__;\
  (R) [7] -= __S0__;\
  (R) [8] += 1.0;\
}

/* partial derivatices of the exponential
 * map => useful for Newton iterations */
#define EXPMAP123(OMEGA, R0, R1, R2)\
{\
  REAL x,\
  a,\
  b,\
  c,\
  b_ac,\
  ac_2cc_2bc,\
  c_b,\
  aux [9],\
  tmp [3];\
  x = DOT(OMEGA, OMEGA);\
  if (x < 1E-16 * DEG_10_SQ)\
  {\
    R0[0]=R0[1]=R0[2]=R0[3]=R0[4]=R0[6]=R0[8]=0;\
    R1[0]=R1[1]=R1[3]=R1[4]=R1[5]=R1[7]=R1[8]=0;\
    R2[0]=R2[2]=R2[4]=R2[5]=R2[6]=R2[7]=R2[8]=0;\
    R0[5]=1.0; R0[7]=-1.0;\
    R1[2]=-1.0; R1[6]=1.0;\
    R2[1]=1.0; R2[3]=-1.0;\
  }\
  else\
  {\
    c = 1.0 / x;\
    if (x < DEG_10_SQ)\
    {\
      a = 1.0 +\
      (-1.666666666666667E-1 +\
       (8.333333333333333E-3 +\
        (-1.984126984126984E-4 +\
         (2.755731922398589E-6 +\
          (-2.505210838544172E-8 +\
           1.605904383682161E-10 * x\
           )* x\
           )* x\
           )* x\
           )* x\
           )* x;\
      b = c - 0.5 +\
      (4.166666666666667E-2 +\
       (-1.388888888888889E-3 +\
        (2.480158730158730E-5 +\
         (-2.755731922398589E-7 +\
          (2.087675698786810E-9 -\
           1.147074559772972E-11 * x\
           )* x\
           )* x\
           )* x\
           )* x\
           )* x;\
    }\
    else\
    {\
      x = sqrt (x);\
      a = sin (x) / x;\
      b = c * cos (x);\
    }\
    b_ac = b - a*c;\
    ac_2cc_2bc = (a - 2.0 * (c - b)) * c;\
    c_b = c - b;\
    DIADIC (OMEGA, OMEGA, aux);\
    SCALE9 (aux, ac_2cc_2bc);\
    COPY (OMEGA, tmp);\
    SCALE (tmp, b_ac);\
    aux [0] -= a;\
    aux [1] += tmp[2];\
    aux [2] -= tmp[1];\
    aux [3] -= tmp[2];\
    aux [4] -=  a;\
    aux [5] += tmp[0];\
    aux [6] += tmp[1];\
    aux [7] -= tmp[0];\
    aux [8] -= a;\
    COPY (OMEGA, tmp);\
    SCALE (tmp, c_b);\
    (R0) [0] = aux[0] * (OMEGA)[0] + 2.0 * tmp[0];\
    (R0) [1] = aux[1] * (OMEGA)[0] + tmp[1];\
    (R0) [2] = aux[2] * (OMEGA)[0] + tmp[2];\
    (R0) [3] = aux[3] * (OMEGA)[0] + tmp[1];\
    (R0) [4] = aux[4] * (OMEGA)[0];\
    (R0) [5] = aux[5] * (OMEGA)[0] + a;\
    (R0) [6] = aux[6] * (OMEGA)[0] + tmp[2];\
    (R0) [7] = aux[7] * (OMEGA)[0] - a;\
    (R0) [8] = aux[8] * (OMEGA)[0];\
    (R1) [0] = aux[0] * (OMEGA)[1];\
    (R1) [1] = aux[1] * (OMEGA)[1] + tmp[0];\
    (R1) [2] = aux[2] * (OMEGA)[1] - a;\
    (R1) [3] = aux[3] * (OMEGA)[1] + tmp[0];\
    (R1) [4] = aux[4] * (OMEGA)[1] + 2.0 * tmp[1];\
    (R1) [5] = aux[5] * (OMEGA)[1] + tmp[2];\
    (R1) [6] = aux[6] * (OMEGA)[1] + a;\
    (R1) [7] = aux[7] * (OMEGA)[1] + tmp[2];\
    (R1) [8] = aux[8] * (OMEGA)[1];\
    (R2) [0] = aux[0] * (OMEGA)[2];\
    (R2) [1] = aux[1] * (OMEGA)[2] + a;\
    (R2) [2] = aux[2] * (OMEGA)[2] + tmp[0];\
    (R2) [3] = aux[3] * (OMEGA)[2] - a;\
    (R2) [4] = aux[4] * (OMEGA)[2];\
    (R2) [5] = aux[5] * (OMEGA)[2] + tmp[1];\
    (R2) [6] = aux[6] * (OMEGA)[2] + tmp[0];\
    (R2) [7] = aux[7] * (OMEGA)[2] + tmp[1];\
    (R2) [8] = aux[8] * (OMEGA)[2] + 2.0 * tmp[2];\
  }\
}

/* compute Mises norm of a Cauchy stress */
#define MISES(s, v)\
  do {\
    REAL a = (s [0] - s [1])*(s [0] - s [1]);\
    REAL b = (s [0] - s [2])*(s [0] - s [2]);\
    REAL c = (s [2] - s [1])*(s [2] - s [1]);\
    REAL d = 6. * (s [3]*s [3] + s [4]*s [4] + s [5]*s [5]);\
    v = .707106781186548 * sqrt (a + b + c + d);\
  } while (0)

#define PROJECT_POINT_ON_LINE(point, line_point, line_direction, projection)\
{\
  REAL dif [3], dot1, dot2, eps;\
  \
  dot1 = DOT (line_direction, line_direction);\
  if (dot1 == 0.0) COPY (line_point, projection);\
  SUB (point, line_point, dif);\
  dot2 = DOT (dif, line_direction);\
  eps = dot2 / dot1;\
  ADDMUL (line_point, eps, line_direction, projection);\
}

#define ROTATION_MATRIX(vector, angle, R)\
{\
  REAL omega [3], a = angle;\
  a *=  ALG_PI / 180.0;\
  COPY (vector, omega);\
  NORMALIZE (omega);\
  SCALE (omega, angle);\
  EXPMAP (omega, R);\
}

#if ISPC
#if REAL_SIZE==4
#define SINCOS(x, s, c) sincos(x, &s, &c)
#else
#define SINCOS(x, s, c) s = sin(x), c = cos(x)
#endif
#else
#define SINCOS(x, s, c) s = sin(x), c = cos(x)
#endif

/* vectorizable exponential map */
#define DEFINE_EXPMAP(TYPE)\
  static inline void expmap (TYPE Omega1, TYPE Omega2, TYPE Omega3,\
      TYPE &Lambda1, TYPE &Lambda2, TYPE &Lambda3,\
      TYPE &Lambda4, TYPE &Lambda5, TYPE &Lambda6,\
      TYPE &Lambda7, TYPE &Lambda8, TYPE &Lambda9)\
{\
  TYPE angsq, sx, cx, v0, v1, v2, v01, v02, v12, s0, s1, s2;\
  \
  v0 = Omega1 * Omega1;\
  v1 = Omega2 * Omega2;\
  v2 = Omega3 * Omega3;\
  \
  angsq = v0 + v1 + v2;\
  \
  if (angsq < 3.0461741978671E-02) /* use Taylor expansion if |Omega| < 10 deg */\
  {\
    sx = 1.0 +\
    (-1.666666666666667E-1 +\
     (8.333333333333333E-3 +\
      (-1.984126984126984E-4 +\
       (2.755731922398589E-6 +\
        (-2.505210838544172E-8 +\
         1.605904383682161E-10 * angsq\
         )*angsq\
         )*angsq\
         )*angsq\
         )*angsq\
         )*angsq;\
    cx = 0.5 +\
    (-4.166666666666667E-2 +\
     (1.388888888888889E-3 +\
      (-2.480158730158730E-5 +\
       (2.755731922398589E-7 +\
        (-2.087675698786810E-9 +\
         1.147074559772972E-11 * angsq\
         )*angsq\
         )*angsq\
         )*angsq\
         )*angsq\
         )*angsq;\
  }\
  else\
  {\
    TYPE t, s, c;\
    t = angsq;\
    angsq = sqrt (angsq);\
    SINCOS (angsq, s, c);\
    sx = s / angsq;\
    cx = (1.0 - c) / t;\
  }\
  \
  v01 = Omega1 * Omega2;\
  v02 = Omega2 * Omega3;\
  v12 = Omega2 * Omega3;\
  s0 = sx * Omega1;\
  s1 = sx * Omega2;\
  s2 = sx * Omega3;\
  \
  Lambda1 = -cx*(v2+v1);\
  Lambda2 = cx*v01;\
  Lambda3 = cx*v02;\
  Lambda4 = Lambda2;\
  Lambda5 = -cx*(v2+v0);\
  Lambda6 = cx*v12;\
  Lambda7 = Lambda3;\
  Lambda8 = Lambda6;\
  Lambda9 = -cx*(v1+v0);\
  Lambda1 += 1.0;\
  Lambda2 += s2;\
  Lambda3 -= s1;\
  Lambda4 -= s2;\
  Lambda5 += 1.0;\
  Lambda6 += s0;\
  Lambda7 += s1;\
  Lambda8 -= s0;\
  Lambda9 += 1.0;\
}

#if ISPC
DEFINE_EXPMAP (uniform REAL);
#endif
DEFINE_EXPMAP (REAL);

/* Given a real symmetric 3x3 matrix A, compute the extremum eigenvalue;
 * Adopted from: https://en.wikipedia.org/wiki/Eigenvalue_algorithm */
#define DEFINE_EXTREMUM_EIGENVALUE(TYPE, INTEGER)\
  inline static TYPE extremum_eigenvalue (TYPE A[9], INTEGER minimum = 0)\
{\
  TYPE eig1, eig2, eig3, ret;\
  \
  TYPE p1 = A[3]*A[3] + A[6]*A[6] + A[7]*A[7];\
  \
  if (p1 == 0.0) /* A is diagonal */\
  {\
    eig1 = A[0];\
    eig2 = A[4];\
    eig3 = A[8];\
    \
    ret = MAX(eig1, eig2);\
    ret = MAX(ret, eig3);\
    return ret;\
  }\
  else\
  {\
    TYPE q = (A[0]+A[4]+A[8])/3.0;\
    TYPE p2 = (A[0]-q)*(A[0]-q) + (A[4]-q)*(A[4]-q) + (A[8]-q)*(A[8]-q) + 2*p1;\
    TYPE p = sqrt(p2 / 6.0);\
    TYPE invp = 1.0 / p;\
    TYPE B[9] = {invp*(A[0]-q), invp*A[1], invp*A[2],\
      invp*A[3], invp*(A[4]-q), invp*A[5],\
      invp*A[6], invp*A[7], invp*(A[8]-q)}; /* (1.0 / p) * (A - q * I), where I is the identity matrix */\
    TYPE r = 0.5 * DET(B);\
    \
    /* In exact arithmetic for a symmetric matrix  -1 <= r <= 1\
     * but computation error can leave it slightly outside this range */\
    TYPE phi;\
    if (r <= -1.0) phi = ALG_PI / 3.0;\
    else if (r >= 1.0) phi = 0.0;\
    else phi = acos(r) / 3.0;\
    \
    /* the eigenvalues satisfy eig3 <= eig2 <= eig1 */\
    if (minimum)\
    {\
      eig3 = q + 2.0 * p * cos(phi + (2*ALG_PI/3));\
      return eig3;\
    }\
    else\
    {\
      eig1 = q + 2.0 * p * cos(phi);\
      return eig1;\
    }\
    /* we skip calculation of eig2: eig2 = 3 * q - eig1 - eig3; since trace(A) = eig1 + eig2 + eig3 */\
  }\
}

#if ISPC
DEFINE_EXTREMUM_EIGENVALUE (REAL, uniform int);
DEFINE_EXTREMUM_EIGENVALUE (uniform REAL, uniform int);
#else
DEFINE_EXTREMUM_EIGENVALUE (REAL, int);
#endif

#endif