alg.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 __alg__
#define __alg__

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

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

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

#ifndef ABS
  #define ABS(v) ((v) > 0 ? (v) : -(v))
#endif

#define ALG_PI 3.14159265358979323846 

/* 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