Any math code out there using column matrices?

Hi,
Is there any (OpenGL) math code (or better yet - a tutorial) I can look at that both:

  1. Uses 4x4 column-major matrices, not row matrices which get transposed before usage.
  2. The 4x4 column-major matrices are defined as an array of 16 floats.

Hence (logically and programmatically) e.g. matrix[2] should be the element at row 3 (2) column 1 (0), not row 1 (0) column 3 (2).

So far I only found code that despite teaching OpenGL uses row matrix notation and transposes it before usage, or where a matrix is an array of arrays.

May be this is useful for You.
There are 2 files (.c and .h) where I have collected some routines to play with vectors and matrices in the last years.
All matrices are 4x4 column-major. When You want to call this routines, You have to declare matrices this way:
my_matrix[16];

The quick and dirty way to use it in Your program:
#include “matrixe.c”

But You can do an object file, or a shared library,
or copy and paste the routine(s) You need in Your file.
I’m not a professional programmer, and all here is very simple “plain” C.
You just have to know a bit about pointers, then You can do what You want.

Here are the 2 files: matrixe.c and matrixe.h

matrixe.c:


/**********************************************
 ***** matrixe.c - Giovanni Rosso         *****
 ***** rogx@libero.it                     *****
 ***** v 0.67 20121118                    *****
 **********************************************
(SORRY FOR MY ENGLISH!)

OpenGl standards: Column direction cosines:

         axs X   axs Y   axs Z    OriginiXYZ
       --------------------------------------
       | cosX    cosX    cosX         X     |
   M = | cosY    cosY    cosY         Y     |
       | cosZ    cosZ    cosZ         Z     |
       |                                    |
       |   0       0       0          1     |
       --------------------------------------

Serialized matrix (by columns)

   The matrix uses the following positions:
       --------------------------------------
       | mat[0]  mat[4]  mat[ 8]    mat[12] |
   M = | mat[1]  mat[5]  mat[ 9]    mat[13] |
       | mat[2]  mat[6]  mat[10]    mat[14] |
       |                                    |
       | mat[3]  mat[7]  mat[11]    mat[15] |
       --------------------------------------
*************************************************************
Projection transformations(gluPerspective, glOrtho,
glMatrixMode(GL_PROJECTION)) are left handed.

Everything else is right handed, including vertexes
(glMatrixMode(GL_MODELVIEW)).
************************************************************
Rem: to call the subroutines that work with matrices declare
     all matrices this way:  double my_matrix[16];

     If in the name of the sub You find "33" the operations will
     use only the orientation part, ie:
     mat[0]  mat[4]  mat[ 8]
     mat[1]  mat[5]  mat[ 9]
     mat[2]  mat[6]  mat[10]

// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];"  (n min 2)
// ***********************************************************************

// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3) 
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0]  [4]  [ 8]
// *******                            [1]  [5]  [ 9]
// *******                            [2]  [6]  [10]
// *****************************************************************

// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4) 
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************

// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************

// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */

#include "matrixe.h" 

static double coef[16] =    /* From mathematica */
  { 1.0, -1.0/2.0, 3.0/8.0, -5.0/16.0, 35.0/128.0, -63.0/256.0,
    231.0/1024.0, -429.0/2048.0, 6435.0/32768.0, -12155.0/65536.0 };

static const double matrix_id[16]=
{1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0};

// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];"  (n min 2)
// ***********************************************************************

// --- returns squared length of input vector: ---    
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_SquaredLength(double *vers, double *VSqLen) 
  {
  *VSqLen = ((vers[0] * vers[0])+(vers[1] * vers[1]));
  }
    
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_Length(double *vers, double *VLen) 
  {
  *VLen = sqrt(((vers[0] * vers[0])+(vers[1] * vers[1])));
  }

// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Add(double *vers_1, double *vers_2, double *vers_r)
  {
  vers_r[0] = vers_1[0] + vers_2[0];
  vers_r[1] = vers_1[1] + vers_2[1];
  }

// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Sub(double *vers_1, double *vers_2, double *vers_r)
  {
  vers_r[0] = vers_1[0] - vers_2[0];
  vers_r[1] = vers_1[1] - vers_2[1];
  }

// --- return the vector perpendicular to the input vector vers_1: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void V2D_MakePerpendicular(double *vers_1, double *vers_r)
  {
  vers_r[0] = -(vers_1[1]);
  vers_r[1] = vers_1[0];
  }

// --- normalizes the input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void NormalizeVers2D(double *vec_1, double *vec_r)
  {
  double k;

  k = 1 / sqrt(vec_1[0] * vec_1[0] + vec_1[1] * vec_1[1] );
  vec_r[0]= vec_1[0] * k;
  vec_r[1]= vec_1[1] * k;
  }// *** End NormalizeVers2D() ***

// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3) 
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0]  [4]  [ 8]
// *******                            [1]  [5]  [ 9]
// *******                            [2]  [6]  [10]
// *****************************************************************

// --- Vector (or XYZ point) * matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XMatr33(double *vec_1, double *mat_1, double *vec_r)
  {
  vec_r[0]=vec_1[0] * mat_1[0] + vec_1[1] * mat_1[4] + vec_1[2] * mat_1[8] ;
  vec_r[1]=vec_1[0] * mat_1[1] + vec_1[1] * mat_1[5] + vec_1[2] * mat_1[9] ;
  vec_r[2]=vec_1[0] * mat_1[2] + vec_1[1] * mat_1[6] + vec_1[2] * mat_1[10];
  }// *** End Vec3XMatr33() ***

// ***************************************************

// --- Vector (or xyz point) * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XtraspMatr33(double *vec_1, double *mat_1, double *vec_r)
  {
  vec_r[0]=vec_1[0] * mat_1[0] + vec_1[1] * mat_1[1] + vec_1[2] * mat_1[2] ;
  vec_r[1]=vec_1[0] * mat_1[4] + vec_1[1] * mat_1[5] + vec_1[2] * mat_1[6] ;
  vec_r[2]=vec_1[0] * mat_1[8] + vec_1[1] * mat_1[9] + vec_1[2] * mat_1[10];
  }// *** End Vec3XtraspMatr33() ***

// ***************************************************

// --- matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrXMatr33(double *mat_1, double *mat_2, double *mat_r)
  {
  mat_r[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[4] + mat_1[2] *mat_2[8] ;
  mat_r[1] = mat_1[0]*mat_2[1] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[9] ;
  mat_r[2] = mat_1[0]*mat_2[2] + mat_1[1]*mat_2[6] + mat_1[2] *mat_2[10];

  mat_r[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[6] *mat_2[8] ;
  mat_r[5] = mat_1[4]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[9] ;
  mat_r[6] = mat_1[4]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[6] *mat_2[10];

  mat_r[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[4] + mat_1[10]*mat_2[8] ;
  mat_r[9] = mat_1[8]*mat_2[1] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[9] ;
  mat_r[10]= mat_1[8]*mat_2[2] + mat_1[9]*mat_2[6] + mat_1[10]*mat_2[10];
  }// *** End MatrXMatr33() ***

// ***************************************************

// --- matrix * transposed matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrXtraspMatr33(double *mat_1, double *mat_2, double *mat_r)
  {
  mat_r[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[1] + mat_1[2] *mat_2[2] ;
  mat_r[1] = mat_1[0]*mat_2[4] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[6] ;
  mat_r[2] = mat_1[0]*mat_2[8] + mat_1[1]*mat_2[9] + mat_1[2] *mat_2[10];

  mat_r[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[1] + mat_1[6] *mat_2[2] ;
  mat_r[5] = mat_1[4]*mat_2[4] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[6] ;
  mat_r[6] = mat_1[4]*mat_2[8] + mat_1[5]*mat_2[9] + mat_1[6] *mat_2[10];

  mat_r[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[1] + mat_1[10]*mat_2[2] ;
  mat_r[9] = mat_1[8]*mat_2[4] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[6] ;
  mat_r[10]= mat_1[8]*mat_2[8] + mat_1[9]*mat_2[9] + mat_1[10]*mat_2[10];
  }// *** End MatrXtraspMatr33() ***

// ***************************************************

// --- transposed matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void TraspMatrXMatr33(double *mat_1, double *mat_2, double *mat_r)
  {
  mat_r[0] = mat_1[0]*mat_2[0] + mat_1[4]*mat_2[4] + mat_1[8] *mat_2[8] ;
  mat_r[1] = mat_1[0]*mat_2[1] + mat_1[4]*mat_2[5] + mat_1[8] *mat_2[9] ;
  mat_r[2] = mat_1[0]*mat_2[2] + mat_1[4]*mat_2[6] + mat_1[8] *mat_2[10];

  mat_r[4] = mat_1[1]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[9] *mat_2[8] ;
  mat_r[5] = mat_1[1]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[9] *mat_2[9] ;
  mat_r[6] = mat_1[1]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[9] *mat_2[10];

  mat_r[8] = mat_1[2]*mat_2[0] + mat_1[6]*mat_2[4] + mat_1[10]*mat_2[8] ;
  mat_r[9] = mat_1[2]*mat_2[1] + mat_1[6]*mat_2[5] + mat_1[10]*mat_2[9] ;
  mat_r[10]= mat_1[2]*mat_2[2] + mat_1[6]*mat_2[6] + mat_1[10]*mat_2[10];
  }// *** End TraspMatrXMatr33() ***

// ***************************************************

// --- Matrix addition: ---
// Matrix addition is commutative.
// Matrix addition is associative.
// This means that ( a + b ) + c = a + ( b + c ).
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrAdd33(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0] + mat_2[0];
  mat_ret[1] = mat_1[1] + mat_2[1];
  mat_ret[2] = mat_1[2] + mat_2[2];
  
  mat_ret[4] = mat_1[4] + mat_2[4];
  mat_ret[5] = mat_1[5] + mat_2[5];
  mat_ret[6] = mat_1[6] + mat_2[6];

  mat_ret[8] = mat_1[8]  + mat_2[8];
  mat_ret[9] = mat_1[9]  + mat_2[9];
  mat_ret[10]= mat_1[10] + mat_2[10];
  }// *** End MatrAdd33() ***

// ***************************************************

// --- Matrix subtraction: ---
// Matrix subtraction is NOT commutative.
// This means that you can't change the order when doing a - b.
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrSubtract33(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0] - mat_2[0];
  mat_ret[1] = mat_1[1] - mat_2[1];
  mat_ret[2] = mat_1[2] - mat_2[2];
  
  mat_ret[4] = mat_1[4] - mat_2[4];
  mat_ret[5] = mat_1[5] - mat_2[5];
  mat_ret[6] = mat_1[6] - mat_2[6];

  mat_ret[8] = mat_1[8]  - mat_2[8];
  mat_ret[9] = mat_1[9]  - mat_2[9];
  mat_ret[10]= mat_1[10] - mat_2[10];
  }// *** End MatrSubtract33() ***

// ***************************************************

// --- transposition: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void trasp_mat33(double *mat_1, double *mat_r)
  {
  mat_r[0]=mat_1[0];   mat_r[4]=mat_1[1];   mat_r[8] =mat_1[2];
  mat_r[1]=mat_1[4];   mat_r[5]=mat_1[5];   mat_r[9] =mat_1[6];
  mat_r[2]=mat_1[8];   mat_r[6]=mat_1[9];   mat_r[10]=mat_1[10];
  }// *** End trasp_mat33() ***

// ***************************************************

// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void scalemul33_ret3(double *scal, double *mat, double *mat_rr)
  {
  mat_rr[0] = mat[0] * *scal;  mat_rr[4] = mat[4] * *scal;  mat_rr[8] = mat[8] * *scal;
  mat_rr[1] = mat[1] * *scal;  mat_rr[5] = mat[5] * *scal;  mat_rr[9] = mat[9] * *scal;
  mat_rr[2] = mat[2] * *scal;  mat_rr[6] = mat[6] * *scal;  mat_rr[10]= mat[10]* *scal;
  }// *** End scalemul33_ret3() ***

// ***************************************************

// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr [0],[4],[8]   | par 1: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]   |                     [1],[5],[9]
//                     [2],[6],[10]  |                     [2],[6],[10]
// par 2: InpVal, var
void scalemul33_ret1(double *mat, double scale)
  {
  mat[ 0] *= scale;  mat[ 4] *= scale;  mat[ 8] *= scale;
  mat[ 1] *= scale;  mat[ 5] *= scale;  mat[ 9] *= scale;
  mat[ 2] *= scale;  mat[ 6] *= scale;  mat[10] *= scale;
  }// *** End scalemul33_ret1() ***

// --- determinant: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
double determinant33(double *mat)
  {
  double value;
  value = mat[2]*mat[4]*mat[ 9]-mat[0]*mat[6]*mat[ 9] -
          mat[2]*mat[5]*mat[ 8]+mat[1]*mat[6]*mat[ 8] -
          mat[1]*mat[4]*mat[10]+mat[0]*mat[5]*mat[10] ;

  return(value);
  }// *** End determinant33() ***

// --- matrix inversion: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void InvMatr33(double *mat, double *mat_ret)
  {
  double det = determinant33(mat);

  mat_ret[0] = mat[ 5]*mat[10] - mat[ 6]*mat[ 9];
  mat_ret[1] = mat[ 2]*mat[ 9] - mat[ 1]*mat[10];
  mat_ret[2] = mat[ 1]*mat[ 6] - mat[ 2]*mat[ 5];

  mat_ret[4] = mat[ 6]*mat[ 8] - mat[ 4]*mat[10];
  mat_ret[5] = mat[ 0]*mat[10] - mat[ 2]*mat[ 8];
  mat_ret[6] = mat[ 2]*mat[ 4] - mat[ 0]*mat[ 6];

  mat_ret[8] = mat[ 4]*mat[ 9] - mat[ 5]*mat[ 8];
  mat_ret[9] = mat[ 1]*mat[ 8] - mat[ 0]*mat[ 9];
  mat_ret[10]= mat[ 0]*mat[ 5] - mat[ 1]*mat[ 4];

  scalemul33_ret1(mat_ret, 1/det);
  }// *** End InvMatr33() ***

// *****************************************************

// --- normalize a vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, vect [0],[1],[2]
void NormalizeVers3D(double *vec_1, double *vec_r)
  {
  double k;

  k = 1 / sqrt(vec_1[0] * vec_1[0] + vec_1[1] * vec_1[1] + vec_1[2] * vec_1[2] );
  vec_r[0]= vec_1[0] * k;
  vec_r[1]= vec_1[1] * k;
  vec_r[2]= vec_1[2] * k;
  }// *** End NormalizeVers3() ***

// ***************************************************

// ***************************************************
// *** 3 routines for matrix orthonormalization:
// *** double matr[16], orientation part:
// *** [0],[4],[8]
// *** [1],[5],[9]
// *** [2],[6],[10]
// *** attention to speed!
// *** benchmarks (1000000 calls):
// *** matr_reortho33     elapsed: 1.618701 --- "best fit"     orthonormalization ---
// *** matr_orthonorm33   elapsed: 0.189059 --- "hierarchical" orthonormalization ---
// *** reorthonormalize33 elapsed: 0.076302 --- "casual"       orthonormalization ---

// --- "best fit" orthonormalization: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void matr_reortho33(double *R, double *mat_ret)
{
/* Eric Raible from "Graphics Gems", Academic Press, 1990
 * 
 * Reorthogonalize matrix R - that is find an orthogonal matrix that is
 * "close" to R by computing an approximation to the orthogonal matrix
 *
 *           T  -1/2
 *   RC = R(R R)
 *               T      -1
 * [RC is orthogonal because (RC) = (CR) ]
 *                                        -1/2
 * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
 * (where x = C - I) about x=0.
 * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
 */

  double I[16], Temp[16], X[16], X_power[16], Sum[16];
  int power;

  TraspMatrXMatr33(R, R, Temp);    /*  RtR  */

  MatrIdentify(I);
  

  MatrSubtract33(Temp, I, X);    /*  RtR - I  */
 
  MatrIdentify(X_power);           /*  X^0  */
  MatrIdentify(Sum);     /*  coef[0] * X^0  */

  for (power = 1; power < 10; power++)
    {
    MatrXMatr33(X_power, X, Temp);
    X_power[0] = Temp[0];   X_power[4] = Temp[4];   X_power[8] = Temp[8];
    X_power[1] = Temp[1];   X_power[5] = Temp[5];   X_power[9] = Temp[9];
    X_power[2] = Temp[2];   X_power[6] = Temp[6];   X_power[10]= Temp[10];

    scalemul33_ret3(&coef[power], X_power, Temp);

    MatrAdd33(Sum, Temp, Sum);
    }

  MatrXMatr33(R, Sum, Temp);

  NormalizeVers3D(Temp, mat_ret);
  NormalizeVers3D(&Temp[4], &mat_ret[4]);
  NormalizeVers3D(&Temp[8], &mat_ret[8]);
}// *** End matr_reortho33() ***

// ***************************************************

// --- "hierarchical" orthonormalization: ---
// --- par. 1, int, is a flag, meaning:
// --- 1=XYZ  2=XZY  3=YZX  4=YXZ  5=ZXY  6=ZYX
// --- (I axs unchanged, II forced to 90 degrees on the plane, III calculated)
// par 1: InpVal, var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void matr_orthonorm33(int xyz123, double *mat_1, double *mat_r)
  {
  /* 1 = XYZ (123)  dx  X<=X Y<=Y Z<=Z. Ret: X<=X  Y<=Y  Z<=Z.
     2 = XZY (132)  sx  X<=X Y<=Z Z<=Y. Ret: X<=X  Y<=-Z Z<=Y.
     3 = YZX (231)  dx  X<=Y Y<=Z Z<=X. Ret: X<=Z  Y<=X  Z<=Y.
     4 = YXZ (213)  sx  X<=Y Y<=X Z<=Z. Ret: X<=Y  Y<=X  Z<=-Z.
     5 = ZXY (312)  dx  X<=Z Y<=X Z<=Y. Ret: X<=Y  Y<=Z  Z<=X.
     6 = ZYX (321)  sx  X<=Z Y<=Y Z<=X. Ret: X<=-Z Y<=Y  Z<=X. */

  double mswp[16];
  double x[3],  y[3],  z[3];
  double x_r[3],y_r[3],z_r[3];

  mswp[0]=0; mswp[4]=0; mswp[1]=0; mswp[5]=0; mswp[2]=0; mswp[6]=0;

  switch(xyz123)
    {
    case 1:
      {
      mswp[0]=mat_1[0] ;  mswp[4]=mat_1[4] ;  mswp[8] =mat_1[8] ;
      mswp[1]=mat_1[1] ;  mswp[5]=mat_1[5] ;  mswp[9] =mat_1[9] ;
      mswp[2]=mat_1[2] ;  mswp[6]=mat_1[6] ;  mswp[10]=mat_1[10];
      break;
      }
    case 2:
      {/*  X <=   X          Y <=   Z          Z <=    Y. Rit: X<=X Y<=-Z Z<=Y. */
      mswp[0]=mat_1[0] ;  mswp[4]=mat_1[8] ;  mswp[8] =mat_1[4] ;
      mswp[1]=mat_1[1] ;  mswp[5]=mat_1[9] ;  mswp[9] =mat_1[5] ;
      mswp[2]=mat_1[2] ;  mswp[6]=mat_1[10];  mswp[10]=mat_1[6] ;
      break;
      }
    case 3:
      {/*  X <=   Y          Y <=   Z          Z <=    X. Rit: X<=Z Y<=X Z<=Y.  */
      mswp[0]=mat_1[4] ;  mswp[4]=mat_1[8] ;  mswp[8] =mat_1[0] ;
      mswp[1]=mat_1[5] ;  mswp[5]=mat_1[9] ;  mswp[9] =mat_1[1] ;
      mswp[2]=mat_1[6] ;  mswp[6]=mat_1[10];  mswp[10]=mat_1[2] ;
      break;
      }
    case 4:
      {/*  X <=   Y          Y <=   X          Z <=    Z. Rit: X<=Y Y<=X Z<=-Z. */
      mswp[0]=mat_1[4] ;  mswp[4]=mat_1[0] ;  mswp[8] =mat_1[8] ;
      mswp[1]=mat_1[5] ;  mswp[5]=mat_1[1] ;  mswp[9] =mat_1[9] ;
      mswp[2]=mat_1[6] ;  mswp[6]=mat_1[2] ;  mswp[10]=mat_1[10];
      break;
      }
    case 5:
      {/*  X <=   Z          Y <=   X          Z <=    Y. Rit:  X<=Y Y<=Z Z<=X  */
      mswp[0]=mat_1[8] ;  mswp[4]=mat_1[0] ;  mswp[8] =mat_1[4];
      mswp[1]=mat_1[9] ;  mswp[5]=mat_1[1] ;  mswp[9] =mat_1[5];
      mswp[2]=mat_1[10];  mswp[6]=mat_1[2] ;  mswp[10]=mat_1[6];
      break;
      }
    case 6:
      {/*  X <=   Z          Y <=   Y          Z <=    X. Rit: X<=-Z Y<=Y Z<=X. */
      mswp[0]=mat_1[8] ;  mswp[4]=mat_1[4] ;  mswp[8] =mat_1[0] ;
      mswp[1]=mat_1[9] ;  mswp[5]=mat_1[5] ;  mswp[9] =mat_1[1] ;
      mswp[2]=mat_1[10];  mswp[6]=mat_1[6] ;  mswp[10]=mat_1[2] ;
      break;
      }
    }

  x[0]= mswp[0];
  x[1]= mswp[1];
  x[2]= mswp[2];

  y[0]= mswp[4];
  y[1]= mswp[5];
  y[2]= mswp[6];

  z[0]= 0.0;
  z[1]= 0.0;
  z[2]= 1.0;

  NormalizeVers3D(x, x_r);
  crossprod(x_r, y, z);
  NormalizeVers3D(z, z_r);
  crossprod(z_r, x_r, y);
  NormalizeVers3D(y, y_r);

  mswp[0]= x_r[0];
  mswp[1]= x_r[1];
  mswp[2]= x_r[2];

  mswp[4]= y_r[0];
  mswp[5]= y_r[1];
  mswp[6]= y_r[2];

  mswp[8]= z_r[0];
  mswp[9]= z_r[1];
  mswp[10]=z_r[2];

  switch(xyz123)
    {
    case 1:
      {
      mat_r[0]=mswp[0] ;  mat_r[4]=mswp[4] ;  mat_r[8] =mswp[8] ;
      mat_r[1]=mswp[1] ;  mat_r[5]=mswp[5] ;  mat_r[9] =mswp[9] ;
      mat_r[2]=mswp[2] ;  mat_r[6]=mswp[6] ;  mat_r[10]=mswp[10];
      break;
      }
    case 2:
      {/* X <=    X         Y <=    Z                Z <=     Y. 
     Rit: X <=    X         Y <=   -Z                Z <=     Y.   */
      mat_r[0]=mswp[0] ;  mat_r[4]=mswp[8]  * -1.0;  mat_r[8] =mswp[4] ;
      mat_r[1]=mswp[1] ;  mat_r[5]=mswp[9]  * -1.0;  mat_r[9] =mswp[5] ;
      mat_r[2]=mswp[2] ;  mat_r[6]=mswp[10] * -1.0;  mat_r[10]=mswp[6] ;
      break;
      }
    case 3:
      {/* X <=    Y         Y <=    Z         Z <=    X. 
     Rit: X <=    Z         Y <=    X         Z <=    Y.    */
      mat_r[0]=mswp[8] ;  mat_r[4]=mswp[0] ;  mat_r[8] =mswp[4] ;
      mat_r[1]=mswp[9] ;  mat_r[5]=mswp[1] ;  mat_r[9] =mswp[5] ;
      mat_r[2]=mswp[10];  mat_r[6]=mswp[2] ;  mat_r[10]=mswp[6] ;
      break;

      }
    case 4:
      {/* X <=   Y          Y <=    X         Z <=     Z. 
     Rit: X <=   Y          Y <=    X         Z <=    -Z. */
      mat_r[0]=mswp[4] ;  mat_r[4]=mswp[0] ;  mat_r[8] =mswp[8]  * -1.0;
      mat_r[1]=mswp[5] ;  mat_r[5]=mswp[1] ;  mat_r[9] =mswp[9]  * -1.0;
      mat_r[2]=mswp[6] ;  mat_r[6]=mswp[2] ;  mat_r[10]=mswp[10] * -1.0;
      break;
      }
    case 5:
      {/* X <=    Z         Y <=    X         Z <=     Y. 
     Rit: X <=    Y         Y <=    Z         Z <=     X    */
      mat_r[0]=mswp[4] ;  mat_r[4]=mswp[8] ;  mat_r[8] =mswp[0];
      mat_r[1]=mswp[5] ;  mat_r[5]=mswp[9] ;  mat_r[9] =mswp[1];
      mat_r[2]=mswp[6] ;  mat_r[6]=mswp[10];  mat_r[10]=mswp[2];
      break;
      }
    case 6:
      {/* X <=    Z               Y <=    Y         Z <=     X. 
     Rit: X <=   -Z               Y <=    Y         Z <=     X.   */
      mat_r[0]=mswp[8] * -1.0;  mat_r[4]=mswp[4] ;  mat_r[8] =mswp[0] ;
      mat_r[1]=mswp[9] * -1.0;  mat_r[5]=mswp[5] ;  mat_r[9] =mswp[1] ;
      mat_r[2]=mswp[10]* -1.0;  mat_r[6]=mswp[6] ;  mat_r[10]=mswp[2];
      break;
      }
    }
  }// *** End matr_orthonorm33() *** 

// ***************************************************

// --- "casual" orthonormalization (best speed): ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 1: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void reorthonormalize33(double *mtrx_REORTH)
  {
  static int vec1ndx, vec2ndx, vec3ndx, lessdev;
  static double axsang[4];

  dotang(mtrx_REORTH, &mtrx_REORTH[4], &axsang[0]);
  dotang(&mtrx_REORTH[4], &mtrx_REORTH[8], &axsang[1]);
  dotang(&mtrx_REORTH[8], mtrx_REORTH, &axsang[2]);

  axsang[0]=fabs(axsang[0] - 90.0);
  axsang[1]=fabs(axsang[1] - 90.0);
  axsang[2]=fabs(axsang[2] - 90.0);

  if((axsang[0]>0.000001) || (axsang[1]>0.000001) ||  (axsang[2]>0.000001))
    {
    lessdev=0;                               // X - Y less deviation
    if (axsang[1]<axsang[0])lessdev=1;       // Y - Z less deviation
    if (axsang[2]<axsang[lessdev])lessdev=2; // Z - X less deviation

    switch (lessdev)
      {
      case 0:
        vec1ndx=0; vec2ndx=4; vec3ndx=8;
        break;
      case 1:
        vec1ndx=4; vec2ndx=8; vec3ndx=0;
        break;
      case 2:
        vec1ndx=8; vec2ndx=0; vec3ndx=4;
      }
    NormalizeVers3D(&mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec1ndx]);
    NormalizeVers3D(&mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec2ndx]);
    crossprod(&mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec3ndx]);
    NormalizeVers3D(&mtrx_REORTH[vec3ndx], &mtrx_REORTH[vec3ndx]);
    crossprod(&mtrx_REORTH[vec3ndx], &mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec2ndx]);
    NormalizeVers3D(&mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec2ndx]);
    }// *** End if((axsang[0]>0.000001) || ... ***
  }// *** End reorthonormalize33() ***

// ***************************************************

// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_SquaredLength(double *vers, double *VSqLen) 
  {
  *VSqLen = ((vers[0] * vers[0]) + (vers[1] * vers[1]) + (vers[2] * vers[2]));
  }

// ***************************************************

// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_Length(double *vers, double *VLen) 
  {
  *VLen = sqrt(((vers[0] * vers[0]) + (vers[1] * vers[1]) + (vers[2] * vers[2])));
  }

// ***************************************************

// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Add(double *vers_1, double *vers_2, double *vers_r)
  {
  vers_r[0] = vers_1[0] + vers_2[0];
  vers_r[1] = vers_1[1] + vers_2[1];
  vers_r[2] = vers_1[2] + vers_2[2];
  }

// ***************************************************

// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Sub(double *vers_1, double *vers_2, double *vers_r)
  {
  vers_r[0] = vers_1[0] - vers_2[0];
  vers_r[1] = vers_1[1] - vers_2[1];
  vers_r[2] = vers_1[2] - vers_2[2];
  }

// ***************************************************

// --- make a linear combination of two vectors and return the result. ---
// --- result = (avect * ascl) + (bvect * bscl) ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, &var
// par 4: InpVal, &var
// par 5: RetVal, vect [0],[1],[2]
void V3D_Combine (double *vers_1, double *vers_2, double *ascl, double *bscl, double *result)
  {
  result[0] = (*ascl * vers_1[0]) + (*bscl * vers_2[0]);
  result[1] = (*ascl * vers_1[1]) + (*bscl * vers_2[1]);
  result[2] = (*ascl * vers_1[2]) + (*bscl * vers_2[2]);
  }

// ***************************************************

// --- Find vector of the segment from point A (xyz) to point B (xyz): ---
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, point [0],[1],[2]
// par 3: RetVal, vect  [0],[1],[2]
void s2vrs(double pt_A[3], double pt_B[3], double *vers)
  {
  double dnmt=0;
  
  dnmt=sqrt((pt_B[0]-pt_A[0])*(pt_B[0]-pt_A[0]) + 
            (pt_B[1]-pt_A[1])*(pt_B[1]-pt_A[1]) +
            (pt_B[2]-pt_A[2])*(pt_B[2]-pt_A[2]));

  vers[0]=(pt_B[0]-pt_A[0])/dnmt;
  vers[1]=(pt_B[1]-pt_A[1])/dnmt;
  vers[2]=(pt_B[2]-pt_A[2])/dnmt;
  }// *** End s2vrs() ***

// ***************************************************

// --- angle between 2 vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotang (double *vec_1, double *vec_2, double *Ang_r)
  {
  *Ang_r = TODEGR * acos((vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]));
  }// *** End dotang() ***

// ***************************************************

// --- like dotang(), but returns angle in radians: ---
void dotang_rad(double *vec_1, double *vec_2, double *Ang_r)
  {
  *Ang_r = acos((vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]));
  }// *** End dotang_rad() ***

// --- cosine of a vector on another vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotprod (double *vec_1, double *vec_2, double *Ang_r)
  {
  *Ang_r = (vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]);
  }// *** End dotprod() ***

// ***************************************************

// --- return a vector normal to the plane of the 2 input vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void crossprod(double *vec_1, double *vec_2, double *vec_r)
  {
  vec_r[0]= vec_1[1] * vec_2[2] - vec_1[2] * vec_2[1];
  vec_r[1]= vec_1[2] * vec_2[0] - vec_1[0] * vec_2[2];
  vec_r[2]= vec_1[0] * vec_2[1] - vec_1[1] * vec_2[0];
  }// *** End crossprod() ***

// ***************************************************

// --- returns the rotation matrix for an angle around X/Y/Z: ---
// par 1: InpVal, &var
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrRotX(double *Ang_1, double *mat_rt )
  {
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat_rt[0]=  1.0 ;  mat_rt[4]= 0.0       ;  mat_rt[8] = 0.0        ;
  mat_rt[1]=  0.0 ;  mat_rt[5]= cos(rdAng);  mat_rt[9] =-sin(rdAng) ;
  mat_rt[2]=  0.0 ;  mat_rt[6]= sin(rdAng);  mat_rt[10]= cos(rdAng) ;
  }// *** End MatrRotX() ***

void MatrRotY(double *Ang_1, double *mat_rt )
  {
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat_rt[0]=  cos(rdAng);  mat_rt[4]= 0.0 ;  mat_rt[8] = sin(rdAng);
  mat_rt[1]=      0.0   ;  mat_rt[5]= 1.0 ;  mat_rt[9] =      0.0   ;
  mat_rt[2]= -sin(rdAng);  mat_rt[6]= 0.0 ;  mat_rt[10]= cos(rdAng);
  }// *** End MatrRotY() ***

void MatrRotZ(double *Ang_1, double *mat_rt )
  {
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat_rt[0]=  cos(rdAng);  mat_rt[4]=-sin(rdAng);  mat_rt[8] = 0.0 ;
  mat_rt[1]=  sin(rdAng);  mat_rt[5]= cos(rdAng);  mat_rt[9] = 0.0 ;
  mat_rt[2]=  0.0       ;  mat_rt[6]= 0.0       ;  mat_rt[10]= 1.0 ;
  }// *** End MatrRotZ() ***

// ***************************************************

// --- rotation of a vector (or a xyz point) around X/Y/Z axs: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void RotVecX(double *Ang_1, double *vec_1, double *vec_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  1.0 ;  mat[4]= 0.0       ;  mat[8] = 0.0        ;
  mat[1]=  0.0 ;  mat[5]= cos(rdAng);  mat[9] =-sin(rdAng) ;
  mat[2]=  0.0 ;  mat[6]= sin(rdAng);  mat[10]= cos(rdAng) ;

  Vec3XMatr33(vec_1, mat, vec_r);
  }// *** End RotVecX() ***

void RotVecY(double *Ang_1, double *vec_1, double *vec_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  cos(rdAng);  mat[4]= 0.0 ;  mat[8] = sin(rdAng);
  mat[1]=      0.0   ;  mat[5]= 1.0 ;  mat[9] =      0.0   ;
  mat[2]= -sin(rdAng);  mat[6]= 0.0 ;  mat[10]= cos(rdAng);

  Vec3XMatr33(vec_1, mat, vec_r);
  }// *** End RotVecY() ***

void RotVecZ(double *Ang_1, double *vec_1, double *vec_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  cos(rdAng);  mat[4]=-sin(rdAng);  mat[8] = 0.0 ;
  mat[1]=  sin(rdAng);  mat[5]= cos(rdAng);  mat[9] = 0.0 ;
  mat[2]=  0.0       ;  mat[6]= 0.0       ;  mat[10]= 1.0 ;

  Vec3XMatr33(vec_1, mat, vec_r);
  }// *** End RotVecZ() ***

// ***************************************************

// --- rotation of a matrix around X/Y/Z axs (only orientation part): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void RotMatrX(double *Ang_1, double *mat_1, double *mat_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  1.0 ;  mat[4]= 0.0       ;  mat[8] = 0.0        ;
  mat[1]=  0.0 ;  mat[5]= cos(rdAng);  mat[9] =-sin(rdAng) ;
  mat[2]=  0.0 ;  mat[6]= sin(rdAng);  mat[10]= cos(rdAng) ;

  MatrXMatr33(mat_1, mat, mat_r);
  }// *** End RotMatrX() ***

void RotMatrY(double *Ang_1, double *mat_1, double *mat_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  cos(rdAng);  mat[4]= 0.0 ;  mat[8] = sin(rdAng);
  mat[1]=      0.0   ;  mat[5]= 1.0 ;  mat[9] =      0.0   ;
  mat[2]= -sin(rdAng);  mat[6]= 0.0 ;  mat[10]= cos(rdAng);

  MatrXMatr33(mat_1, mat, mat_r);
  }// *** End RotMatrY() ***

void RotMatrZ(double *Ang_1, double *mat_1, double *mat_r)
  {
  double mat[16];
  double rdAng=0.0;

  rdAng = *Ang_1 * TORAD;
  mat[0]=  cos(rdAng);  mat[4]=-sin(rdAng);  mat[8] = 0.0 ;
  mat[1]=  sin(rdAng);  mat[5]= cos(rdAng);  mat[9] = 0.0 ;
  mat[2]=  0.0       ;  mat[6]= 0.0       ;  mat[10]= 1.0 ;

  MatrXMatr33(mat_1, mat, mat_r);
  }// *** End RotMatrZ() ***

// ***************************************************

// --- linear interpolation between vectors: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: RetVal, vect [0],[1],[2]
void Vec3Lerp(double *lerp, double *vec_1, double *vec_2, double *vec_r)
{
   vec_r[0] = vec_1[0] + (*lerp) * (vec_2[0] - vec_1[0]);
   vec_r[1] = vec_1[1] + (*lerp) * (vec_2[1] - vec_1[1]);
   vec_r[2] = vec_1[2] + (*lerp) * (vec_2[2] - vec_1[2]);
}// *** End Vec3Lerp() ***

// ***************************************************

/* If You want to call Matr33Slerp() o QuatSlerp()
   You have to allocate a "MatrixSlerp" struct:

typedef struct
  {
  double mat_from[16];
  double mat_to[16];
  double quat_from[4];
  double quat_to[4];
  double percent;
  double quat_r[4];
  double mat_r[16];
  }MatrixSlerp;         */

// --- spherical interpolation between matrices: ---
// par 1: allocate a "MatrixSlerp" struct
void Matr33Slerp(MatrixSlerp *mslerp)
  {

  Matr33ToQuat(mslerp->mat_from, mslerp->quat_from);
  Matr33ToQuat(mslerp->mat_to, mslerp->quat_to);
  QuatSlerp(mslerp);
  QuatToMatr33(mslerp->quat_r, mslerp->mat_r);
  }// *** End Matr33Slerp() ***

// --- spherical interpolation between quaternions: ---
// par 1: allocate a "MatrixSlerp" struct
void QuatSlerp(MatrixSlerp *qslerp)
  {

  double to1[4];
  double omega, cosom, sinom, scale0, scale1;

  // calc cosine
  cosom = qslerp->quat_from[0] * qslerp->quat_to[0] + 
          qslerp->quat_from[1] * qslerp->quat_to[1] + 
          qslerp->quat_from[2] * qslerp->quat_to[2] + 
   qslerp->quat_from[3] * qslerp->quat_to[3];

  // adjust signs (if necessary)
  if ( cosom <0.0 )
    {
    cosom = -cosom;
    to1[0] = - qslerp->quat_to[0];
    to1[1] = - qslerp->quat_to[1];
    to1[2] = - qslerp->quat_to[2];
    to1[3] = - qslerp->quat_to[3];
    }
  else
    {
    to1[0] = qslerp->quat_to[0];
    to1[1] = qslerp->quat_to[1];
    to1[2] = qslerp->quat_to[2];
    to1[3] = qslerp->quat_to[3];
    }

    // calculate coefficients

    if ( (1.0 - cosom) > 0.00001 ) // if ((1.0 - cosom) > DELTA)
      {                            // standard case (slerp)
      omega = acos(cosom);
      sinom = sin(omega);
      scale0 = sin((1.0 - qslerp->percent) * omega) / sinom;
      scale1 = sin(qslerp->percent * omega) / sinom;
      }
    else // "from" and "to" quaternions are very close 
      {  //  ... so we can do a linear interpolation       
      scale0 = 1.0 - qslerp->percent;
      scale1 = qslerp->percent;
      }
    // calculate final values
    qslerp->quat_r[0] = scale0 * qslerp->quat_from[0] + scale1 * to1[0];
    qslerp->quat_r[1] = scale0 * qslerp->quat_from[1] + scale1 * to1[1];
    qslerp->quat_r[2] = scale0 * qslerp->quat_from[2] + scale1 * to1[2];
    qslerp->quat_r[3] = scale0 * qslerp->quat_from[3] + scale1 * to1[3];
  } // *** End QuatSlerp() ***

// ***************************************************

// --- from direct cosines matrix to Euler angles: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2]
void Dcm33ToEul(double *mat, double *eulang)
 {
/* Assuming the angles are in radians.
   Kuka robots standard:

    |z       (seq: first A around Z, second B around Y, then C around X:
    |       -------------------
    |_____    eulang[0] = C - (X)   
    /  y    eulang[1] = B - (Y)   
   /       eulang[2] = A - (Z)   
  x       ------------------- */
    double C=0, tr_x=0, tr_y=0;
    eulang[1]  = asin( mat[2] );       // *** Calculate Y-axis angle *** 
    C        = cos ( eulang[1]);
    eulang[1] *= TODEGR;
    if ( fabs( C ) > 0.0005 )          // *** Gimball lock? ***
      {
      tr_x      =  mat[10] / C;        // *** No, so get X-axis angle ***
      tr_y      = -mat[6]  / C;
      eulang[0]  = atan2( tr_y, tr_x ) * TODEGR;
      tr_x      =  mat[0] / C;         // *** Get Z-axis angle ***
      tr_y      = -mat[1] / C;
      eulang[2]  = atan2( tr_y, tr_x ) * TODEGR;
      }
    else                               // *** Gimball lock has occurred ***
      {
      printf("
Gimball lock !!");
      eulang[0]  = 0;                  // *** Set X-axis angle to zero ***
      tr_x      = mat[5];              // *** And calculate Z-axis angle ***
      tr_y      = mat[4];
      eulang[2]  = atan2( tr_y, tr_x ) * TODEGR;
      }

  eulang[2] = -eulang[2]; // Minus sign is
  eulang[1] = -eulang[1]; // ok for KUKA - CATIA
  eulang[0] = -eulang[0]; // and for CATIA - KUKA

  // * return only positive angles in [0,360] *
  // *    if (eulang[0] < 0) eulang[0] += 360;
  // *    if (eulang[1] < 0) eulang[1] += 360;
  // *    if (eulang[2] < 0) eulang[2] += 360;
  } // *** End Dcm33ToEul() ***

// ***************************************************

// --- from Euler angles to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void EulToDcm33(double *eulang, double *mat_ret1)
  {
  //  Kuka robots standard: (seq: first A around Z, second B around Y, then C around X:
  // eulang[0] = C - (X)
  // eulang[1] = B - (Y)
  // eulang[2] = A - (Z)

  double mat_axs1[16], mat_ret2[16];
  double Ang1=0;
  
  mat_axs1[0]= 1.0 ;  mat_axs1[4]= 0.0 ;  mat_axs1[8] = 0.0 ;  mat_axs1[12]= 0.0 ;
  mat_axs1[1]= 0.0 ;  mat_axs1[5]= 1.0 ;  mat_axs1[9] = 0.0 ;  mat_axs1[13]= 0.0 ;
  mat_axs1[2]= 0.0 ;  mat_axs1[6]= 0.0 ;  mat_axs1[10]= 1.0 ;  mat_axs1[14]= 0.0 ;
  mat_axs1[3]= 0.0 ;  mat_axs1[7]= 0.0 ;  mat_axs1[11]= 0.0 ;  mat_axs1[15]= 1.0 ;

  Ang1=-eulang[2];
  RotMatrZ(&Ang1, mat_axs1, mat_ret2);

  Ang1=-eulang[1];
  RotMatrY(&Ang1, mat_ret2, mat_ret1);

  Ang1=-eulang[0];
  RotMatrX(&Ang1, mat_ret1, mat_ret2);

  trasp_mat33(mat_ret2, mat_ret1);
  } // *** End EulToDcm33() ***

// ***************************************************

// --- from direct cosines matrix to quaternions: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2],[3]  declare: "double miovers[n];" (n min 4)
void Matr33ToQuat(double *mat, double *quat)
  {
  // ******* ABB robot Q1 (the scalar) is quat[3] of this routine! *******
  // *** Q2 => quat[0] *** Q3 => quat[1] *** Q4 => quat[2] ***

  double trace, s;

  trace = 1.0 + mat[0] + mat[5] + mat[10];

  if (trace > 0.00000001)
    {                                 //printf("trace > 0.0
");
    s = 0.5  / sqrt(trace);
    quat[3] = 0.25 / s;

    quat[0] = (mat[6] - mat[9]) * s;
    quat[1] = (mat[8] - mat[2]) * s;
    quat[2] = (mat[1] - mat[4]) * s;
    }
  else
    {
    if((mat[0]>mat[5]) && (mat[0]>mat[10]))
      {                               //printf("trace <= 0.0, col0
");
      s = sqrt(1.0 + mat[0] - mat[5] - mat[10]) * 2.0;
      quat[0] = s * 0.25;
      quat[1] = (mat[1] + mat[4]) / s;
      quat[2] = (mat[2] + mat[8]) / s;
      quat[3] = (mat[9] - mat[6]) / s;
      }
    else if(mat[5]>mat[10])
      {                               //printf("trace <= 0.0, col1
");
      s = sqrt(1.0 + mat[5] - mat[0] - mat[10]) * 2.0;
      quat[0] = (mat[1] + mat[4]) / s;
      quat[1] = s * 0.25;
      quat[2] = (mat[6] + mat[9]) / s;
      quat[3] = (mat[8] - mat[2]) / s;
      }
    else
      {                               //printf("trace <= 0.0, col2
");
      s = sqrt(1.0 + mat[10] - mat[0] - mat[5]) * 2.0;
      quat[0] = (mat[2] + mat[8]) / s;
      quat[1] = (mat[6] + mat[9]) / s;
      quat[2] = s * 0.25;
      quat[3] = (mat[4] - mat[1]) / s;
      }
    }
  }// *** End Matr33ToQuat() ***

// ***************************************************

// --- from quaternions to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2],[3] declare.: "double my_vect[n];" (n min 4)
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void QuatToMatr33(double *quat, double *mat)
  {
  double  xx = quat[0]*quat[0],
          yy = quat[1]*quat[1],
          zz = quat[2]*quat[2],

          xy = quat[0]*quat[1],
          xz = quat[0]*quat[2],
          yz = quat[1]*quat[2],

          xw = quat[0]*quat[3],
          yw = quat[1]*quat[3],
          zw = quat[2]*quat[3];

    mat[0] = 1.0 - 2.0 * (yy + zz);
    mat[4] = 2.0 * (xy - zw);
    mat[8] = 2.0 * (xz + yw);
    mat[12]= 0.0;

    mat[1] = 2.0 * (xy + zw);
    mat[5] = 1.0 - 2.0 * (xx + zz);
    mat[9] = 2.0 * (yz - xw);
    mat[13]= 0.0;

    mat[2] = 2.0 * (xz - yw);
    mat[6] = 2.0 * (yz + xw);
    mat[10]= 1.0 - 2.0 * (xx + yy);
    mat[14]= 0.0;

    mat[3] = 0.0;
    mat[7] = 0.0;
    mat[11]= 0.0;
    mat[15]= 1.0;
  } // *** End QuatToMatr33() *** 

// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* declare vec as: double my_vec[n]; (n min 4) 
// ******* declare matrix as: double my_matrix[16];
// ******* all matrix will be used.
// *****************************************************************

// --- vector * matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XMatr44(double *vec_i, double *matr, double *vec_r)
  {
  vec_r[0]=vec_i[0] * matr[0] + vec_i[1] * matr[4] + vec_i[2] * matr[8]  + vec_i[3] * matr[12];
  vec_r[1]=vec_i[0] * matr[1] + vec_i[1] * matr[5] + vec_i[2] * matr[9]  + vec_i[3] * matr[13];
  vec_r[2]=vec_i[0] * matr[2] + vec_i[1] * matr[6] + vec_i[2] * matr[10] + vec_i[3] * matr[14];
  vec_r[3]=vec_i[0] * matr[3] + vec_i[1] * matr[7] + vec_i[2] * matr[11] + vec_i[3] * matr[15];
  }/*** End Vec4XMatr44() ***/

// ***************************************************

// --- vector * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XtraspMatr44(double *vec_i, double *matr, double *vec_r)
  {
  vec_r[0]=vec_i[0] * matr[0]  + vec_i[1] * matr[1]  + vec_i[2] * matr[2]  + vec_i[3] * matr[3] ;
  vec_r[1]=vec_i[0] * matr[4]  + vec_i[1] * matr[5]  + vec_i[2] * matr[6]  + vec_i[3] * matr[7] ;
  vec_r[2]=vec_i[0] * matr[8]  + vec_i[1] * matr[9]  + vec_i[2] * matr[10] + vec_i[3] * matr[11];
  vec_r[3]=vec_i[0] * matr[12] + vec_i[1] * matr[13] + vec_i[2] * matr[14] + vec_i[3] * matr[15];
  }// *** End Vec4XtraspMatr44() ***

// ***************************************************

// --- matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXMatr44(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[4] + mat_1[2] *mat_2[8]  + mat_1[3] *mat_2[12];
  mat_ret[1] = mat_1[0]*mat_2[1] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[9]  + mat_1[3] *mat_2[13];
  mat_ret[2] = mat_1[0]*mat_2[2] + mat_1[1]*mat_2[6] + mat_1[2] *mat_2[10] + mat_1[3] *mat_2[14];
  mat_ret[3] = mat_1[0]*mat_2[3] + mat_1[1]*mat_2[7] + mat_1[2] *mat_2[11] + mat_1[3] *mat_2[15];
  
  mat_ret[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[6] *mat_2[8]  + mat_1[7] *mat_2[12];
  mat_ret[5] = mat_1[4]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[9]  + mat_1[7] *mat_2[13];
  mat_ret[6] = mat_1[4]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[6] *mat_2[10] + mat_1[7] *mat_2[14];
  mat_ret[7] = mat_1[4]*mat_2[3] + mat_1[5]*mat_2[7] + mat_1[6] *mat_2[11] + mat_1[7] *mat_2[15];

  mat_ret[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[4] + mat_1[10]*mat_2[8]  + mat_1[11]*mat_2[12];
  mat_ret[9] = mat_1[8]*mat_2[1] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[9]  + mat_1[11]*mat_2[13];
  mat_ret[10]= mat_1[8]*mat_2[2] + mat_1[9]*mat_2[6] + mat_1[10]*mat_2[10] + mat_1[11]*mat_2[14];
  mat_ret[11]= mat_1[8]*mat_2[3] + mat_1[9]*mat_2[7] + mat_1[10]*mat_2[11] + mat_1[11]*mat_2[15];

  mat_ret[12]= mat_1[12]*mat_2[0]+ mat_1[13]*mat_2[4]+ mat_1[14]*mat_2[8]  + mat_1[15]*mat_2[12];
  mat_ret[13]= mat_1[12]*mat_2[1]+ mat_1[13]*mat_2[5]+ mat_1[14]*mat_2[9]  + mat_1[15]*mat_2[13];
  mat_ret[14]= mat_1[12]*mat_2[2]+ mat_1[13]*mat_2[6]+ mat_1[14]*mat_2[10] + mat_1[15]*mat_2[14];
  mat_ret[15]= mat_1[12]*mat_2[3]+ mat_1[13]*mat_2[7]+ mat_1[14]*mat_2[11] + mat_1[15]*mat_2[15];
  }// *** End MatrXMatr44() ***

// ***************************************************

// --- matrix * transposed matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXtraspMatr44(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[1]  + mat_1[2] *mat_2[2] + mat_1[3] *mat_2[3] ;
  mat_ret[1] = mat_1[0]*mat_2[4] + mat_1[1]*mat_2[5]  + mat_1[2] *mat_2[6] + mat_1[3] *mat_2[7] ;
  mat_ret[2] = mat_1[0]*mat_2[8] + mat_1[1]*mat_2[9]  + mat_1[2] *mat_2[10]+ mat_1[3] *mat_2[11];
  mat_ret[3] = mat_1[0]*mat_2[12]+ mat_1[1]*mat_2[13] + mat_1[2] *mat_2[14]+ mat_1[3] *mat_2[15];
  
  mat_ret[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[1]  + mat_1[6] *mat_2[2] + mat_1[7] *mat_2[3] ;
  mat_ret[5] = mat_1[4]*mat_2[4] + mat_1[5]*mat_2[5]  + mat_1[6] *mat_2[6] + mat_1[7] *mat_2[7] ;
  mat_ret[6] = mat_1[4]*mat_2[8] + mat_1[5]*mat_2[9]  + mat_1[6] *mat_2[10]+ mat_1[7] *mat_2[11];
  mat_ret[7] = mat_1[4]*mat_2[12]+ mat_1[5]*mat_2[13] + mat_1[6] *mat_2[14]+ mat_1[7] *mat_2[15];

  mat_ret[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[1]  + mat_1[10]*mat_2[2] + mat_1[11]*mat_2[3] ;
  mat_ret[9] = mat_1[8]*mat_2[4] + mat_1[9]*mat_2[5]  + mat_1[10]*mat_2[6] + mat_1[11]*mat_2[7] ;
  mat_ret[10]= mat_1[8]*mat_2[8] + mat_1[9]*mat_2[9]  + mat_1[10]*mat_2[10]+ mat_1[11]*mat_2[11];
  mat_ret[11]= mat_1[8]*mat_2[12]+ mat_1[9]*mat_2[13] + mat_1[10]*mat_2[14]+ mat_1[11]*mat_2[15];

  mat_ret[12]=mat_1[12]*mat_2[0] + mat_1[13]*mat_2[1] + mat_1[14]*mat_2[2] + mat_1[15]*mat_2[3] ;
  mat_ret[13]=mat_1[12]*mat_2[4] + mat_1[13]*mat_2[5] + mat_1[14]*mat_2[6] + mat_1[15]*mat_2[7] ;
  mat_ret[14]=mat_1[12]*mat_2[8] + mat_1[13]*mat_2[9] + mat_1[14]*mat_2[10]+ mat_1[15]*mat_2[11];
  mat_ret[15]=mat_1[12]*mat_2[12]+ mat_1[13]*mat_2[13]+ mat_1[14]*mat_2[14]+ mat_1[15]*mat_2[15];
  }// *** End MatrXtraspMatr44() ***

// ***************************************************

// --- transposed matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void TraspMatrXMatr44(double *mat_1, double *mat_2, double *mat_r)
  {
  mat_r[0] = mat_1[0]*mat_2[0] + mat_1[4]*mat_2[4] + mat_1[8] *mat_2[8]  + mat_1[12]*mat_2[12];
  mat_r[1] = mat_1[0]*mat_2[1] + mat_1[4]*mat_2[5] + mat_1[8] *mat_2[9]  + mat_1[12]*mat_2[13];
  mat_r[2] = mat_1[0]*mat_2[2] + mat_1[4]*mat_2[6] + mat_1[8] *mat_2[10] + mat_1[12]*mat_2[14];
  mat_r[3] = mat_1[0]*mat_2[3] + mat_1[4]*mat_2[7] + mat_1[8] *mat_2[11] + mat_1[12]*mat_2[15];

  mat_r[4] = mat_1[1]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[9] *mat_2[8]  + mat_1[13]*mat_2[12];
  mat_r[5] = mat_1[1]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[9] *mat_2[9]  + mat_1[13]*mat_2[13];
  mat_r[6] = mat_1[1]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[9] *mat_2[10] + mat_1[13]*mat_2[14];
  mat_r[7] = mat_1[1]*mat_2[3] + mat_1[5]*mat_2[7] + mat_1[9] *mat_2[11] + mat_1[13]*mat_2[15];

  mat_r[8] = mat_1[2]*mat_2[0] + mat_1[6]*mat_2[4] + mat_1[10]*mat_2[8]  + mat_1[14]*mat_2[12];
  mat_r[9] = mat_1[2]*mat_2[1] + mat_1[6]*mat_2[5] + mat_1[10]*mat_2[9]  + mat_1[14]*mat_2[13];
  mat_r[10]= mat_1[2]*mat_2[2] + mat_1[6]*mat_2[6] + mat_1[10]*mat_2[10] + mat_1[14]*mat_2[14];
  mat_r[11]= mat_1[2]*mat_2[3] + mat_1[6]*mat_2[7] + mat_1[10]*mat_2[11] + mat_1[14]*mat_2[15];

  mat_r[12]= mat_1[3]*mat_2[0] + mat_1[7]*mat_2[4] + mat_1[11]*mat_2[8]  + mat_1[15]*mat_2[12];
  mat_r[13]= mat_1[3]*mat_2[1] + mat_1[7]*mat_2[5] + mat_1[11]*mat_2[9]  + mat_1[15]*mat_2[13];
  mat_r[14]= mat_1[3]*mat_2[2] + mat_1[7]*mat_2[6] + mat_1[11]*mat_2[10] + mat_1[15]*mat_2[14];
  mat_r[15]= mat_1[3]*mat_2[3] + mat_1[7]*mat_2[7] + mat_1[11]*mat_2[11] + mat_1[15]*mat_2[15];
  }// *** End TraspMatrXMatr44() ***

// ***************************************************

// --- Matrix addition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrAdd44(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0] + mat_2[0];
  mat_ret[1] = mat_1[1] + mat_2[1];
  mat_ret[2] = mat_1[2] + mat_2[2];
  mat_ret[3] = mat_1[3] + mat_2[3];
  
  mat_ret[4] = mat_1[4] + mat_2[4];
  mat_ret[5] = mat_1[5] + mat_2[5];
  mat_ret[6] = mat_1[6] + mat_2[6];
  mat_ret[7] = mat_1[7] + mat_2[7];

  mat_ret[8] = mat_1[8]  + mat_2[8];
  mat_ret[9] = mat_1[9]  + mat_2[9];
  mat_ret[10]= mat_1[10] + mat_2[10];
  mat_ret[11]= mat_1[11] + mat_2[11];

  mat_ret[12]= mat_1[12] + mat_2[12];
  mat_ret[13]= mat_1[13] + mat_2[13];
  mat_ret[14]= mat_1[14] + mat_2[14];
  mat_ret[15]= mat_1[15] + mat_2[15];
  }// *** End MatrAdd44() ***

// ***************************************************

// --- Matrix subtraction: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrSubtract44(double *mat_1, double *mat_2, double *mat_ret)
  {
  mat_ret[0] = mat_1[0] - mat_2[0];
  mat_ret[1] = mat_1[1] - mat_2[1];
  mat_ret[2] = mat_1[2] - mat_2[2];
  mat_ret[3] = mat_1[3] - mat_2[3];
  
  mat_ret[4] = mat_1[4] - mat_2[4];
  mat_ret[5] = mat_1[5] - mat_2[5];
  mat_ret[6] = mat_1[6] - mat_2[6];
  mat_ret[7] = mat_1[7] - mat_2[7];

  mat_ret[8] = mat_1[8]  - mat_2[8];
  mat_ret[9] = mat_1[9]  - mat_2[9];
  mat_ret[10]= mat_1[10] - mat_2[10];
  mat_ret[11]= mat_1[11] - mat_2[11];

  mat_ret[12]= mat_1[12] - mat_2[12];
  mat_ret[13]= mat_1[13] - mat_2[13];
  mat_ret[14]= mat_1[14] - mat_2[14];
  mat_ret[15]= mat_1[15] - mat_2[15];
  }// *** End MatrSubtract44() ***

// ***************************************************

// --- transposition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void trasp_mat44(double *mat_1, double *mat_r)
  {
  mat_r[0]=mat_1[0];   mat_r[4]=mat_1[1];   mat_r[8] =mat_1[2] ;  mat_r[12]=mat_1[3] ;
  mat_r[1]=mat_1[4];   mat_r[5]=mat_1[5];   mat_r[9] =mat_1[6] ;  mat_r[13]=mat_1[7] ;
  mat_r[2]=mat_1[8];   mat_r[6]=mat_1[9];   mat_r[10]=mat_1[10];  mat_r[14]=mat_1[11];
  mat_r[3]=mat_1[12];  mat_r[7]=mat_1[13];  mat_r[11]=mat_1[14];  mat_r[15]=mat_1[15];
  }// *** End trasp_mat44() ***

// ***************************************************

// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void scalemul44_ret3(double *scal, double *mat, double *mat_rr)
  {// --- (var di ritorno III par) ---
  mat_rr[0] = mat[0] * *scal; mat_rr[4] = mat[4] * *scal; mat_rr[8] = mat[8] * *scal; mat_rr[12]= mat[12]* *scal;
  mat_rr[1] = mat[1] * *scal; mat_rr[5] = mat[5] * *scal; mat_rr[9] = mat[9] * *scal; mat_rr[13]= mat[13]* *scal;
  mat_rr[2] = mat[2] * *scal; mat_rr[6] = mat[6] * *scal; mat_rr[10]= mat[10]* *scal; mat_rr[14]= mat[14]* *scal;
  mat_rr[3] = mat[3] * *scal; mat_rr[7] = mat[7] * *scal; mat_rr[11]= mat[11]* *scal; mat_rr[15]= mat[15]* *scal;
  }// *** End scalemul44_ret3() ***

// ***************************************************

// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr[16] (column matr)  |  par 1: RetVal, matr[16] (column matr)
// par 2: InpVal, var
void scalemul44_ret1(double *mat, double scale)
  {// --- (var di ritorno I par) ---
  mat[ 0] *= scale;  mat[ 4] *= scale;  mat[ 8] *= scale;  mat[12] *= scale;
  mat[ 1] *= scale;  mat[ 5] *= scale;  mat[ 9] *= scale;  mat[13] *= scale;
  mat[ 2] *= scale;  mat[ 6] *= scale;  mat[10] *= scale;  mat[14] *= scale;
  mat[ 3] *= scale;  mat[ 7] *= scale;  mat[11] *= scale;  mat[15] *= scale;
  }// *** End scalemul44() ***

// --- determinant: ---
// par 1: InpVal, matr[16] (column matr)
double determinant44(double *mat)
  {
  double value;
  value =
  mat[3]*mat[6]*mat[ 9]*mat[12]-mat[2]*mat[7]*mat[ 9]*mat[12]-mat[3]*mat[5]*mat[10]*mat[12]+mat[1]*mat[7]*mat[10]*mat[12]+
  mat[2]*mat[5]*mat[11]*mat[12]-mat[1]*mat[6]*mat[11]*mat[12]-mat[3]*mat[6]*mat[ 8]*mat[13]+mat[2]*mat[7]*mat[ 8]*mat[13]+
  mat[3]*mat[4]*mat[10]*mat[13]-mat[0]*mat[7]*mat[10]*mat[13]-mat[2]*mat[4]*mat[11]*mat[13]+mat[0]*mat[6]*mat[11]*mat[13]+
  mat[3]*mat[5]*mat[ 8]*mat[14]-mat[1]*mat[7]*mat[ 8]*mat[14]-mat[3]*mat[4]*mat[ 9]*mat[14]+mat[0]*mat[7]*mat[ 9]*mat[14]+
  mat[1]*mat[4]*mat[11]*mat[14]-mat[0]*mat[5]*mat[11]*mat[14]-mat[2]*mat[5]*mat[ 8]*mat[15]+mat[1]*mat[6]*mat[ 8]*mat[15]+
  mat[2]*mat[4]*mat[ 9]*mat[15]-mat[0]*mat[6]*mat[ 9]*mat[15]-mat[1]*mat[4]*mat[10]*mat[15]+mat[0]*mat[5]*mat[10]*mat[15];
  return(value);
  }// *** End determinant44() ***

// --- matrix inversion: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void InvMatr44(double *mat, double *mat_ret)
  {
  double det = determinant44(mat);

  mat_ret[0] = mat[ 6]*mat[11]*mat[13] - mat[ 7]*mat[10]*mat[13] + mat[ 7]*mat[ 9]*mat[14] - mat[ 5]*mat[11]*mat[14] - mat[ 6]*mat[ 9]*mat[15] + mat[ 5]*mat[10]*mat[15];
  mat_ret[1] = mat[ 3]*mat[10]*mat[13] - mat[ 2]*mat[11]*mat[13] - mat[ 3]*mat[ 9]*mat[14] + mat[ 1]*mat[11]*mat[14] + mat[ 2]*mat[ 9]*mat[15] - mat[ 1]*mat[10]*mat[15];
  mat_ret[2] = mat[ 2]*mat[ 7]*mat[13] - mat[ 3]*mat[ 6]*mat[13] + mat[ 3]*mat[ 5]*mat[14] - mat[ 1]*mat[ 7]*mat[14] - mat[ 2]*mat[ 5]*mat[15] + mat[ 1]*mat[ 6]*mat[15];
  mat_ret[3] = mat[ 3]*mat[ 6]*mat[ 9] - mat[ 2]*mat[ 7]*mat[ 9] - mat[ 3]*mat[ 5]*mat[10] + mat[ 1]*mat[ 7]*mat[10] + mat[ 2]*mat[ 5]*mat[11] - mat[ 1]*mat[ 6]*mat[11];

  mat_ret[4] = mat[ 7]*mat[10]*mat[12] - mat[ 6]*mat[11]*mat[12] - mat[ 7]*mat[ 8]*mat[14] + mat[ 4]*mat[11]*mat[14] + mat[ 6]*mat[ 8]*mat[15] - mat[ 4]*mat[10]*mat[15];
  mat_ret[5] = mat[ 2]*mat[11]*mat[12] - mat[ 3]*mat[10]*mat[12] + mat[ 3]*mat[ 8]*mat[14] - mat[ 0]*mat[11]*mat[14] - mat[ 2]*mat[ 8]*mat[15] + mat[ 0]*mat[10]*mat[15];
  mat_ret[6] = mat[ 3]*mat[ 6]*mat[12] - mat[ 2]*mat[ 7]*mat[12] - mat[ 3]*mat[ 4]*mat[14] + mat[ 0]*mat[ 7]*mat[14] + mat[ 2]*mat[ 4]*mat[15] - mat[ 0]*mat[ 6]*mat[15];
  mat_ret[7] = mat[ 2]*mat[ 7]*mat[ 8] - mat[ 3]*mat[ 6]*mat[ 8] + mat[ 3]*mat[ 4]*mat[10] - mat[ 0]*mat[ 7]*mat[10] - mat[ 2]*mat[ 4]*mat[11] + mat[ 0]*mat[ 6]*mat[11];

  mat_ret[8] = mat[ 5]*mat[11]*mat[12] - mat[ 7]*mat[ 9]*mat[12] + mat[ 7]*mat[ 8]*mat[13] - mat[ 4]*mat[11]*mat[13] - mat[ 5]*mat[ 8]*mat[15] + mat[ 4]*mat[ 9]*mat[15];
  mat_ret[9] = mat[ 3]*mat[ 9]*mat[12] - mat[ 1]*mat[11]*mat[12] - mat[ 3]*mat[ 8]*mat[13] + mat[ 0]*mat[11]*mat[13] + mat[ 1]*mat[ 8]*mat[15] - mat[ 0]*mat[ 9]*mat[15];
  mat_ret[10]= mat[ 1]*mat[ 7]*mat[12] - mat[ 3]*mat[ 5]*mat[12] + mat[ 3]*mat[ 4]*mat[13] - mat[ 0]*mat[ 7]*mat[13] - mat[ 1]*mat[ 4]*mat[15] + mat[ 0]*mat[ 5]*mat[15];
  mat_ret[11]= mat[ 3]*mat[ 5]*mat[ 8] - mat[ 1]*mat[ 7]*mat[ 8] - mat[ 3]*mat[ 4]*mat[ 9] + mat[ 0]*mat[ 7]*mat[ 9] + mat[ 1]*mat[ 4]*mat[11] - mat[ 0]*mat[ 5]*mat[11];

  mat_ret[12]= mat[ 6]*mat[ 9]*mat[12] - mat[ 5]*mat[10]*mat[12] - mat[ 6]*mat[ 8]*mat[13] + mat[ 4]*mat[10]*mat[13] + mat[ 5]*mat[ 8]*mat[14] - mat[ 4]*mat[ 9]*mat[14];
  mat_ret[13]= mat[ 1]*mat[10]*mat[12] - mat[ 2]*mat[ 9]*mat[12] + mat[ 2]*mat[ 8]*mat[13] - mat[ 0]*mat[10]*mat[13] - mat[ 1]*mat[ 8]*mat[14] + mat[ 0]*mat[ 9]*mat[14];
  mat_ret[14]= mat[ 2]*mat[ 5]*mat[12] - mat[ 1]*mat[ 6]*mat[12] - mat[ 2]*mat[ 4]*mat[13] + mat[ 0]*mat[ 6]*mat[13] + mat[ 1]*mat[ 4]*mat[14] - mat[ 0]*mat[ 5]*mat[14];
  mat_ret[15]= mat[ 1]*mat[ 6]*mat[ 8] - mat[ 2]*mat[ 5]*mat[ 8] + mat[ 2]*mat[ 4]*mat[ 9] - mat[ 0]*mat[ 6]*mat[ 9] - mat[ 1]*mat[ 4]*mat[10] + mat[ 0]*mat[ 5]*mat[10];  

  scalemul44_ret1(mat_ret, 1/det);
  }// *** End InvMatr44() ***

// ******************************************************

// --- normalize a vector (ok also for quaternions): ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: RetVal, vect [0],[1],[2],[3]
void NormalizeVers4(double *quat, double *vec_r)
  {
  double length, ilength;

   length = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3] ;
   length = sqrt(length);

   if ( length > 0.0)
     {
     ilength = 1.0/length;
     vec_r[0] = quat[0] * ilength;
     vec_r[1] = quat[1] * ilength;
     vec_r[2] = quat[2] * ilength;
     vec_r[3] = quat[3] * ilength;
     }
  }// *** End NormalizeVers4() ***

// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************

// --- direct transformation of a vector: (Vec3XtraspMatr33) ---
//  par 1: ImpVal, vec [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------- cosines ----
//                 matr [0],[4],[8] 
//                      [1],[5],[9] 
//                      [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
/* #define transf_dir_vec33 Vec3XtraspMatr33 --- in matrixe.h --- */

// --- inverse transformation of a vector: (Vec3XMatr33) ---
//  par 1: ImpVal, vec [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------- cosines ----
//                 matr [0],[4],[8] 
//                      [1],[5],[9] 
//                      [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
/* #define transf_inv_vec33 Vec3XMatr33 --- in matrixe.h --- */

// --- direct transformation of a point: (sub, Vec3XtraspMatr33) ---
//  par 1: ImpVal, point [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------ cosines --|-- ori (xyz) --
//                 matr [0],[4],[8] ,    [12]
//                      [1],[5],[9] ,    [13]
//                      [2],[6],[10],    [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_dir_pt331(double *pt_input, double *matrix, double *pt_return)
  {
  double pt_temp[4];
  
  pt_temp[0]=pt_input[0] - matrix[12];
  pt_temp[1]=pt_input[1] - matrix[13];
  pt_temp[2]=pt_input[2] - matrix[14];
  Vec3XtraspMatr33(pt_temp, matrix, pt_return);
  }// *** End transf_dir_pt3() ***

// --- inverse transformation of a point: (Vec3XMatr33, add) ---
//  par 1: ImpVal, point [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------ cosines --|-- ori (xyz) --
//                 matr [0],[4],[8] ,    [12]
//                      [1],[5],[9] ,    [13]
//                      [2],[6],[10],    [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_inv_pt331(double *pt_input, double *matrix, double *pt_return)
  {
  double pt_temp[4];

  Vec3XMatr33(pt_input, matrix, pt_temp);
  pt_return[0]=pt_temp[0] + matrix[12];
  pt_return[1]=pt_temp[1] + matrix[13];
  pt_return[2]=pt_temp[2] + matrix[14];
  }// *** End transf_inv_pt3() ***

// --- direct transformation of a matrix: MatrXtraspMatr33,
//                                           (sub, Vec3XtraspMatr33) ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: InpVal, matr[16]
//  par 3: RetVal, matr[16]
void transf_dir_axs331(double *matrix1, double *matrix2, double *matrix_ret)
  {
  double pt_temp[4];

  MatrXtraspMatr33(matrix1, matrix2, matrix_ret);
  transf_dir_pt331(&matrix1[12], matrix2, pt_temp);
  matrix_ret[12]=pt_temp[0]; matrix_ret[13]=pt_temp[1]; matrix_ret[14]=pt_temp[2];
  }// *** End transf_dir_axs33() ***

// --- inverse transformation of a matrix: MatrXMatr33,
//                                           (Vec3XMatr33, add) ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: InpVal, matr[16]
//  par 3: RetVal, matr[16]
void transf_inv_axs331(double *matrix1, double *matrix2, double *matrix_ret)
  {
  double pt_temp[4];

  MatrXMatr33(matrix1, matrix2, matrix_ret);
  transf_inv_pt331(&matrix1[12], matrix2, pt_temp);
  matrix_ret[12]=pt_temp[0]; matrix_ret[13]=pt_temp[1]; matrix_ret[14]=pt_temp[2];
  }// *** End transf_inv_axs33() ***

// --- Invert definition of an axis system: ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: RetVal, matr[16]
void inv_axs_def331(double *matrix, double *matrix_ret)
  {
  transf_dir_axs331((double *)matrix_id, matrix, matrix_ret);
  }// *** End transf_dir_axs33() ***

// --- return identity matrix: ---
//  par 1: InpVal, matrix declared matr[16]
//                 matr [0],[4],[8] ,[12]
//                      [1],[5],[9] ,[13]
//                      [2],[6],[10],[14]
//                      [3],[7],[11],[15]
//
//  par 1: RetVal, 1   0   0   0
//      0   1   0   0
//      0   0   1   0
//                 0   0   0   1
void MatrIdentify( double *matrix )
  {
  matrix[0] = 1.0;  matrix[4] = 0.0;  matrix[8]  = 0.0;  matrix[12] = 0.0;
  matrix[1] = 0.0;  matrix[5] = 1.0;  matrix[9]  = 0.0;  matrix[13] = 0.0;
  matrix[2] = 0.0;  matrix[6] = 0.0;  matrix[10] = 1.0;  matrix[14] = 0.0;
  matrix[3] = 0.0;  matrix[7] = 0.0;  matrix[11] = 0.0;  matrix[15] = 1.0;
  }// *** End MatrIdentify() ***

// --- print on console (printf()) a matrix: ---
// --- mode(par1): 1) 3x3,col   2) 3x3,row   3) 4x4,col   4) 4x4,row
// matrix declared matr[16]
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, matr [0],[4],[8] 
//                     [1],[5],[9] 
//                     [2],[6],[10]
// If par 1 = 3 o par 1 = 4  par 2 will be all the matrix[16]
void print_matr(int mode, double *mat)
  {// mode: 1) 3x3,col   2) 3x3,row   3) 4x4,col   4) 4x4,row
  switch(mode)
    {
    case 1:   // 3x3,col
      {
      printf("(column vers)
");
      printf(" %.10f,      %.10f,      %.10f
", mat[0], mat[4], mat[8]);
      printf(" %.10f,      %.10f,      %.10f
", mat[1], mat[5], mat[9]);
      printf(" %.10f,      %.10f,      %.10f
", mat[2], mat[6], mat[10]);
      break;
      }
    case 2:   // 3x3,row
      {
      printf("(row vers)
");
      printf(" %.10f,      %.10f,      %.10f
", mat[0], mat[1], mat[2]);
      printf(" %.10f,      %.10f,      %.10f
", mat[4], mat[5], mat[6]);
      printf(" %.10f,      %.10f,      %.10f
", mat[8], mat[9], mat[10]);
      break;
      }
    case 3:   // 4x4,col
      {
      printf("(column vers)
");
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[0], mat[4], mat[8] , mat[12]);
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[1], mat[5], mat[9] , mat[13]);
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[2], mat[6], mat[10], mat[14]);
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[3], mat[7], mat[11], mat[15]);
      break;
      }
    case 4:   // 4x4,row
      {
      printf("(row vers)
");
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[0] , mat[1] , mat[2] , mat[3] );
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[4] , mat[5] , mat[6] , mat[7] );
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[8] , mat[9] , mat[10], mat[11]);
      printf(" %.10f,      %.10f,      %.10f,      %.10f
", mat[12], mat[13], mat[14], mat[15]);
      break;
      }
    }// *** End switch(mode) ***
  }// *** End print_matr() ***

// ***************************************************

// --- print on console (printf()) a vector (3 o 4 elements)
// --- mode(par1): 1) vect3,col   2) vect3,row   3) vect4,col   4) vect4,row
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, vect [0],[1],[2]
// If par 1 = 3 o par 1 = 4  par 2 will be all vect[4]
void print_vers(int mode, double *vers)
  {// mode: 1) 3,col   2) 3,row   3) 4,col   4) 4,row
  switch(mode)
    {
    case 1:   // 3,col
      {
      printf(" %.10f
 %.10f
 %.10f

", vers[0], vers[1], vers[2]);
      break;
      }
    case 2:   // 3,row
      {
      printf(" %.10f,    %.10f,    %.10f

", vers[0], vers[1], vers[2]);
      break;
      }
    case 3:   // 4,col
      {
      printf(" %.10f
 %.10f
 %.10f
 %.10f

", vers[0], vers[1], vers[2], vers[3]);
      break;
      }
    case 4:   // 4,row
      {
      printf(" %.10f,    %.10f,    %.10f,    %.10f

", vers[0], vers[1], vers[2], vers[3]);
      break;
      }
    }// *** End switch(mode) ***
  }// *** End print_vers() ***

// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */

// --- distance between 2 points xy(2D): ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, &var
void dist_pt2pt2(double *pt_1, double *pt_2, double *distptpt)
  {
  *distptpt = sqrt((pt_2[0]-pt_1[0])*(pt_2[0]-pt_1[0]) + (pt_2[1]-pt_1[1])*(pt_2[1]-pt_1[1]));
  }// *** End dist_pt2pt2() ***

// ***************************************************

// --- distance between 2 points xyz(3D): ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dist_pt3pt3(double *pt_1, double *pt_2, double *distptpt)
  {
  *distptpt = sqrt((pt_2[0]-pt_1[0])*(pt_2[0]-pt_1[0]) +
                   (pt_2[1]-pt_1[1])*(pt_2[1]-pt_1[1]) +
     (pt_2[2]-pt_1[2])*(pt_2[2]-pt_1[2]));
  }// *** End dist_pt3pt3() ***

// ***************************************************

// --- intersection of 2 circ(2D): 1° circ: x,y center and radius.
// ---                             2° circ: x,y center and radius.
// --- Return in x1,y1 e x2,y2 ---
// par 1: InpVal, &var   |   par 1: RetVal, &var
// par 2: InpVal, &var   |   par 2: RetVal, &var
// par 3: InpVal, var
// par 4: InpVal, &var   |   par 4: RetVal, &var
// par 5: InpVal, &var   |   par 5: RetVal, &var
// par 6: InpVal, var
int c2c (double *x1, double *y1, double r1, double *x2, double *y2, double r2)
  {
  double xx1=0, yy1=0, xx2=0, yy2=0;
  double rmax=0,rmin=0;
  double cdist=0;
  double vara=0,varh=0,varpX=0,varpY=0,varb=0;
  
  xx1=*x1;yy1=*y1;xx2=*x2;yy2=*y2;
  
  rmax=r1+r2;
  rmin=fabs(r1-r2);
  cdist=sqrt((xx2-xx1)*(xx2-xx1) + (yy2-yy1)*(yy2-yy1));
  
  if(cdist>rmax)
    return 1;
  else if(cdist<rmin)
    return 2;
  else
    {
    vara=(r1 * r1 - r2 * r2 + cdist*cdist) / (2 * cdist);
    varh=sqrt(r1 * r1 - vara * vara);
    
    //lerp:
    varpX=(xx1 + (xx2 - xx1) * (vara/cdist));
    varpY=(yy1 + (yy2 - yy1) * (vara/cdist));
    
    varb=varh/cdist;
    
    *x1=varpX - varb * (yy2 - yy1);
    *y1=varpY + varb * (xx2 - xx1);
    
    *x2=varpX + varb * (yy2 - yy1);
    *y2=varpY - varb * (xx2 - xx1);

    return 0;
    }
  }// *** End c2c() ***

// ***************************************************

// --- triangle angles, if we know sides. ---
// par 1: InpVal, var
// par 2: InpVal, var
// par 3: InpVal, var
// par 4: RetVal, vect [0],[1],[2]
void tri(double lato_a, double lato_b, double lato_c, double *angles )
  {/*
               /|
              / |
             /ta|
            /   |
           /    |
         c/     |b
         /      |
        /       |
       /tb    tc|
       ----------
            a         */
    
  angles[0]=TODEGR * acos((lato_b*lato_b+lato_c*lato_c-lato_a*lato_a)/(2*lato_b*lato_c)); // ta
  angles[1]=TODEGR * acos((lato_a*lato_a+lato_c*lato_c-lato_b*lato_b)/(2*lato_a*lato_c)); // tb
  angles[2]=TODEGR * acos((lato_a*lato_a+lato_b*lato_b-lato_c*lato_c)/(2*lato_a*lato_b)); // tc
  }// *** End tri() ***
  
// ***************************************************

// --- 4 points on a sphere, returns center (xyz) and radius: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: InpVal, vect [0],[1],[2]
// par 5: RetVal, vect [0],[1],[2]
// par 6: RetVal, &var
int sfera(double *pt_1, double *pt_2, double *pt_3, double *pt_4, double *ptc, double *radius)
  {
  double sfmat[16];
  double m11,m12,m13,m14,m15;

  /* Find determinant M11 */
  sfmat[0] = pt_1[0]; sfmat[4] = pt_1[1]; sfmat[ 8] = pt_1[2]; sfmat[12] = 1;
  sfmat[1] = pt_2[0]; sfmat[5] = pt_2[1]; sfmat[ 9] = pt_2[2]; sfmat[13] = 1;
  sfmat[2] = pt_3[0]; sfmat[6] = pt_3[1]; sfmat[10] = pt_3[2]; sfmat[14] = 1;
  sfmat[3] = pt_4[0]; sfmat[7] = pt_4[1]; sfmat[11] = pt_4[2]; sfmat[15] = 1;
  m11 = determinant44(sfmat);

  /* Find determinant M12 */    
  sfmat[0] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
  sfmat[4] = pt_1[1];
  sfmat[8] = pt_1[2];
  sfmat[12]= 1;
  sfmat[1] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
  sfmat[5] = pt_2[1];
  sfmat[9] = pt_2[2];
  sfmat[13]= 1;
  sfmat[2] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
  sfmat[6] = pt_3[1];
  sfmat[10]= pt_3[2];
  sfmat[14]= 1;
  sfmat[3] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
  sfmat[7] = pt_4[1];
  sfmat[11]= pt_4[2];
  sfmat[15]= 1;
  m12  = determinant44(sfmat);

  /* Find determinant M13 */    
  sfmat[0] = pt_1[0];
  sfmat[4] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
  sfmat[8] = pt_1[2];
  sfmat[12]= 1;
  sfmat[1] = pt_2[0];
  sfmat[5] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
  sfmat[9] = pt_2[2];
  sfmat[13]= 1;
  sfmat[2] = pt_3[0];
  sfmat[6] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
  sfmat[10]= pt_3[2];
  sfmat[14]= 1;
  sfmat[3] = pt_4[0];
  sfmat[7] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
  sfmat[11]= pt_4[2];
  sfmat[15]= 1;
  m13  = determinant44(sfmat);

  /* Find determinant M14 */    
  sfmat[0] = pt_1[0];
  sfmat[4] = pt_1[1];
  sfmat[8] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
  sfmat[12]= 1;
  sfmat[1] = pt_2[0];
  sfmat[5] = pt_2[1];
  sfmat[9] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
  sfmat[13]= 1;
  sfmat[2] = pt_3[0];
  sfmat[6] = pt_3[1];
  sfmat[10]= pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
  sfmat[14]= 1;
  sfmat[3] = pt_4[0];
  sfmat[7] = pt_4[1];
  sfmat[11]= pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
  sfmat[15]= 1;
  m14  = determinant44(sfmat);

  /* Find determinant M15 */    
  sfmat[0] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
  sfmat[4] = pt_1[0];
  sfmat[8] = pt_1[1];
  sfmat[12]= pt_1[2];
  sfmat[1] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
  sfmat[5] = pt_2[0];
  sfmat[9] = pt_2[1];
  sfmat[13]= pt_2[2];
  sfmat[2] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
  sfmat[6] = pt_3[0];
  sfmat[10]= pt_3[1];
  sfmat[14]= pt_3[2];
  sfmat[3] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
  sfmat[7] = pt_4[0];
  sfmat[11]= pt_4[1];
  sfmat[15]= pt_4[2];
  m15  = determinant44(sfmat);

  fprintf(stderr,"Determinants: %g %g %g %g %g
",m11,m12,m13,m14,m15);
  if (m11 == 0) {
     fprintf(stderr,"The points don't define a sphere!
");
     ptc[0] = 0.0;
     ptc[1] = 0.0;
     ptc[2] = 0.0;
     *radius = 0.0;
     return(1);
  }

  ptc[0] = 0.5 * m12 / m11;
  ptc[1] = 0.5 * m13 / m11;
  ptc[2] = 0.5 * m14 / m11;
  *radius = sqrt(ptc[0]*ptc[0] + ptc[1]*ptc[1] + ptc[2]*ptc[2] - m15/m11);
  fprintf(stderr,"Sphere  Center: %g,%g,%g  Radius: %g
", ptc[0], ptc[1], ptc[2], *radius); 

  return(0);
  }// *** End sfera() ***

// ***************************************************

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- distance point - line (3d). ---
// double line[8];
// par 1: InpVal, point [0],[1],[2] 
// par 2: InpVal, line  [0],[1],[2] --- start pt
//                      [4],[5],[6] --- end pt
// par 3: RetVal, var               --- distance
// par 4: RetVal, vect [0],[1],[2]  --- pt on the line
void dist_Point_to_Line( double *P, double *L, double *dist, double *line_pt)
  {
  double vecV[4], vecW[4];
  double cos1, cos2, b;

  vecV[0] = L[4]-L[0];
  vecV[1] = L[5]-L[1];
  vecV[2] = L[6]-L[2];

  vecW[0] = P[0]-L[0];
  vecW[1] = P[1]-L[1];
  vecW[2] = P[2]-L[2];

  dotprod(vecW, vecV, &cos1);
  dotprod(vecV, vecV, &cos2);
  b = cos1 / cos2;

  line_pt[0] = L[0]+ b * vecV[0];
  line_pt[1] = L[1]+ b * vecV[1];
  line_pt[2] = L[2]+ b * vecV[2];

  dist_pt3pt3(P, line_pt, dist);
  }// *** End dist_Point_to_Line() ***

// ***************************************************

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// get base of perpendicular from point to a plane
//   Input:  P = a 3D point
//           PL = a plane with point V0 and normal n
//   Output: *B(pt_on_pln) = base point on PL of perpendicular from P
//   Return: the distance from P to the plane PL
//
// double plane[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, plane [0],[1],[2] x,y,z pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2]
// par 4: RetVal, var               distance
void dist_Point_to_Plane( double *P, double *PL, double *pt_on_pln, double *dst)
  {
  double cos1, cos2, sb;
  double pt_t[4];

  pt_t[0] = P[0] - PL[0];
  pt_t[1] = P[1] - PL[1];
  pt_t[2] = P[2] - PL[2];
  dotprod(&PL[4], pt_t, &cos1);
  dotprod(&PL[4], &PL[4], &cos2);
  sb = (-cos1) / cos2;

  pt_on_pln[0] = P[0]+ sb * PL[4];
  pt_on_pln[1] = P[1]+ sb * PL[5];
  pt_on_pln[2] = P[2]+ sb * PL[6];

  dist_pt3pt3(P, pt_on_pln, dst);
}// *** End dist_Point_to_Plane() ***

// ***************************************************

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- Input:  two 3D lines L1 and L2 --- Return: the shortest distance ---
// double line[8]; // [0],[1],[2] x,y,z start pt    [4],[5],[6] end pt
// par 1: InpVal, line [0],[1],[2] start pt
//                     [4],[5],[6] end pt
// par 2: InpVal, line [0],[1],[2]
//                     [4],[5],[6] end pt
// par 3: RetVal, &var
// par 4: RetVal, &var
// par 5: RetVal, point [0],[1],[2]
// par 6: RetVal, point [0],[1],[2]
//    Return: the shortest distance between L1 and L2,
//            lines parameters in vL1par and vL2par
//            and nearest points in pt_1 and pt_2
double dist3D_Line_to_Line( double *vL1, double *vL2, double *vL1par, double *vL2par, double *pt_1,  double *pt_2)
  {
  double vec_u[4], vec_v[4], vec_w[4], dP[4], vec_tmp1[4], vec_tmp2[4], vec_tmp3[4];
  double a,b,c,d,e,D,retv;
  double  vL1len, vL1vers[4];
  double  vL2len, vL2vers[4];

  V3D_Sub(&vL1[4], vL1, vec_u);
  V3D_Sub(&vL2[4], vL2, vec_v);
  V3D_Sub(vL1, vL2, vec_w);

  dotprod(vec_u, vec_u, &a);       // always >= 0
  dotprod(vec_u, vec_v, &b);
  dotprod(vec_v, vec_v, &c);       // always >= 0
  dotprod(vec_u, vec_w, &d);
  dotprod(vec_v, vec_w, &e);
  D = a*c - b*b;                   // always >= 0

  // compute the line parameters of the two closest points
  if (D < SMALL_NUM) {             // the lines are almost parallel
      *vL1par = 0.0;
      *vL2par = (b>c ? d/b : e/c); // use the largest denominator
  }
  else {
      *vL1par = (b*e - c*d) / D;
      *vL2par = (a*e - b*d) / D;
  }

  dist_pt3pt3(vL1, &vL1[4], &vL1len);
  dist_pt3pt3(vL2, &vL2[4], &vL2len);

  vL1len = vL1len * (*vL1par); // distance start pt - nearest pt
  vL2len = vL2len * (*vL2par);

  s2vrs(vL1, &vL1[4], vL1vers);
  s2vrs(vL2, &vL2[4], vL2vers);

  pt_1[0] = vL1[0] + vL1vers[0]*vL1len;  pt_1[1] = vL1[1] + vL1vers[1]*vL1len;  pt_1[2] = vL1[2] + vL1vers[2]*vL1len;
  pt_2[0] = vL2[0] + vL2vers[0]*vL2len;  pt_2[1] = vL2[1] + vL2vers[1]*vL2len;  pt_2[2] = vL2[2] + vL2vers[2]*vL2len;

  // get the difference of the two closest points
  /// Vector   dP = w + (sc * u) - (tc * v);  // = L1(sc) - L2(tc)

  vec_tmp1[0] = *vL1par * vec_u[0]; vec_tmp1[1] = *vL1par * vec_u[1]; vec_tmp1[2] = *vL1par * vec_u[2];
  V3D_Add(vec_w, vec_tmp1, vec_tmp2);
  vec_tmp3[0] = *vL2par * vec_v[0]; vec_tmp3[1] = *vL2par * vec_v[1]; vec_tmp3[2] = *vL2par * vec_v[2];
  V3D_Sub(vec_tmp2, vec_tmp3, dP);

  V3D_Length(dP, &retv);
  return retv;
}// *** End dist3D_Line_to_Line() ***

// ***************************************************

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_SegmentPlane(): intersect a segment and a plane
//    Input:  S = a segment, and Pn = a plane = {Point V0; Vector n;}
//    Output: *I0 = the intersect point (when it exists)
//    Return: 0 = disjoint (no intersection)
//            1 = intersection in the unique point *I0
//            2 = the segment lies in the plane
//
// double segm[8]; double plan[8]; double pnt[4]; int *retval;
// par 1: InpVal,  line [0],[1],[2] start pt
//                      [4],[5],[6] end pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2] inters. point
// par 4: RetVal, &var              return flag
void inters_ln_pln( double *segm, double *plan, double *pnt, int *retval)
  {
  double u[4];
  double v[4];
  double D, N, interspar;

  u[0] = segm[4] - segm[0];  u[1] = segm[5] - segm[1];  u[2] = segm[6] - segm[2];
  v[0] = segm[0] - plan[0];  v[1] = segm[1] - plan[1];  v[2] = segm[2] - plan[2];

  dotprod(&plan[4], u, &D);
  dotprod(&plan[4], v, &N);

    if (fabs(D) < 0.000000001) // segment is parallel to plane
      {
        if (N == 0)            // segment lies in plane
            *retval = 2;
        else
            *retval = 0;       // no intersection
    }

  // they are not parallel
  // compute intersect param
  interspar = (-N) / D;

    if (interspar < 0 || interspar > 1)
        *retval = 0;           // no intersection

  // compute segment intersect point
  pnt[0] = segm[0] + interspar * u[0];
  pnt[1] = segm[1] + interspar * u[1];
  pnt[2] = segm[2] + interspar * u[2];

    *retval = 1;
}// *** End inters_ln_pln() ***

// ***************************************************

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_2Planes(): the 3D intersect of two planes
//    Input:  two planes Pn1 and Pn2
//    Output: *L = the intersection line (when it exists)
//    Return: 0 = disjoint (no intersection)
//            1 = the two planes coincide
//            2 = intersection in the unique line *L
// par 1: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, line  [0],[1],[2] start pt
//                      [4],[5],[6] end pt
// par 4: RetVal, &var              return flag
void inters_pln_pln(double *pln1, double *pln2, double *ln, int *retval)
  {
  double u[4], v[4];
  double ax, ay, az, cos1;
  double d1, d2;          // the constants in the 2 plane equations
  int    maxc;            // max coordinate

  crossprod(&pln1[4], &pln2[4], u);

  ax = (u[0] >= 0 ? u[0] : -u[0]);
  ay = (u[1] >= 0 ? u[1] : -u[1]);
  az = (u[2] >= 0 ? u[2] : -u[2]);

  // test if the two planes are parallel
  if ((ax+ay+az) < 0.000000001) // Pn1 and Pn2 are near parallel
    {
    // test if disjoint or coincide
    v[0] = pln2[0] - pln1[0];
    v[1] = pln2[1] - pln1[1];
    v[2] = pln2[2] - pln1[2];

    dotprod(&pln1[4], v, &cos1);
    if(cos1 == 0)  // pln2 pt lies in pln1
      *retval = 1; // pln1 and pln2 coincide
    else
      *retval = 0; // pln1 and pln2 are disjoint
    }

  // Pn1 and Pn2 intersect in a line
  // first determine max abs coordinate of cross product
  if (ax > ay)
    {
    if (ax > az) maxc = 1;
    else  maxc = 3;
    }
  else
    {
    if (ay > az) maxc = 2;
    else  maxc = 3;
    }

  // next, to get a point on the intersect line
  // zero the max coord, and solve for the other two
  dotprod(&pln1[4], pln1, &d1); d1 = -d1; // note: could be pre-stored with plane
  dotprod(&pln2[4], pln2, &d2); d2 = -d2; // ditto
  switch (maxc)    // select max coordinate
    {
    case 1:  // intersect with x=0 (ln[0], [1], [2]  intersection point)
      ln[0] = 0;
      ln[1] = (d2*pln1[6] - d1*pln2[6]) / u[0];
      ln[2] = (d1*pln2[5] - d2*pln1[5]) / u[0];
      break;
    case 2:  // intersect with y=0
      ln[0] = (d1*pln2[6] - d2*pln1[6]) / u[1];
      ln[1] = 0;
      ln[2] = (d2*pln1[4] - d1*pln2[4]) / u[1];
      break;
    case 3:  // intersect with z=0
      ln[0] = (d2*pln1[5] - d1*pln2[5]) / u[2];
      ln[1] = (d1*pln2[4] - d2*pln1[4]) / u[2];
      ln[2] = 0;
    }
  ln[4] = ln[0] + u[0];  ln[5] = ln[1] + u[1];  ln[6] = ln[2] + u[2];
  *retval = 2;
}// *** End inters_pln_pln() ***

// --- Geometric center of gravity of a triangle, knowing the vertices: ---
// par 1: InpVal, pt [0],[1],[2]
// par 2: InpVal, pt [0],[1],[2]
// par 3: RetVal, pt [0],[1],[2]
// par 4: RetVal, pt [0],[1],[2]
void tri_centroid(double *pt_1, double *pt_2, double *pt_3, double *pt_return)
  {
  pt_return[0] = (pt_1[0] + pt_2[0] + pt_3[0]) / 3.0;
  pt_return[1] = (pt_1[1] + pt_2[1] + pt_3[1]) / 3.0;
  pt_return[2] = (pt_1[2] + pt_2[2] + pt_3[2]) / 3.0;
  }// *** End tri_centroid() ***

// ***************************************************

// ***************************************************
// ***************************************************

/***** Serialization - deSerialization: **************
       --------------------------------------------
       | mat[0]     mat[4]     mat[ 8]    mat[12] |
   M = | mat[1]     mat[5]     mat[ 9]    mat[13] |
       | mat[2]     mat[6]     mat[10]    mat[14] |
       | mat[3]     mat[7]     mat[11]    mat[15] |
       --------------------------------------------

       ----------------------------------------------
       | mat[0][0]  mat[0][1]  mat[0][2]  mat[0][3] |
   M = | mat[1][0]  mat[1][1]  mat[1][2]  mat[1][3] |
       | mat[2][0]  mat[2][1]  mat[2][2]  mat[2][3] |
       | mat[3][0]  mat[3][1]  mat[3][2]  mat[3][3] |
       ----------------------------------------------

The doubly dimensioned array A is treated as a one dimensional
vector, stored by COLUMNS.  Entry A(I,J) is stored as A[I+J*N]

*****************************************************/

and matrixe.h:


/***********************************************
 ***** matrixe.h - Giovanni Rosso          *****
 ***** rogx@libero.it                      *****
 ***** for matrixe.c  v 0.67 20121118      *****
************************************************
(SORRY FOR MY ENGLISH!)

OpenGl standards: Column direction cosines:

         axs X   axs Y   axs Z    OriginiXYZ
       --------------------------------------
       | cosX    cosX    cosX         X     |
   M = | cosY    cosY    cosY         Y     |
       | cosZ    cosZ    cosZ         Z     |
       |                                    |
       |   0       0       0          1     |
       --------------------------------------

Serialized matrix (by columns)

   The matrix uses the following positions:
       --------------------------------------
       | mat[0]  mat[4]  mat[ 8]    mat[12] |
   M = | mat[1]  mat[5]  mat[ 9]    mat[13] |
       | mat[2]  mat[6]  mat[10]    mat[14] |
       |                                    |
       | mat[3]  mat[7]  mat[11]    mat[15] |
       --------------------------------------
*************************************************************
Projection transformations(gluPerspective, glOrtho,
glMatrixMode(GL_PROJECTION)) are left handed.

Everything else is right handed, including vertexes
(glMatrixMode(GL_MODELVIEW)).
************************************************************
Rem: to call the subroutines that work with matrices declare
     all matrices this way:  double my_matrix[16];

     If in the name of the sub You find "33" the operations will
     use only the orientation part, ie:
     mat[0]  mat[4]  mat[ 8]
     mat[1]  mat[5]  mat[ 9]
     mat[2]  mat[6]  mat[10]

// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];"  (n min 2)
// ***********************************************************************

// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3) 
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0]  [4]  [ 8]
// *******                            [1]  [5]  [ 9]
// *******                            [2]  [6]  [10]
// *****************************************************************

// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4) 
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************

// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************

// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */

#include <stdio.h>
#include <math.h>

#define TODEGR 57.295779513082322864647721871733665466308593750
#define TORAD   0.01745329251994329547437168059786927187815308570861816406250
//              3.14159265358979323846264338327950288419716939937510582097494459230781640628620899862803482534211706
#define PGRECO  3.14159265358979323846264338327950288419716939937510582097494
#define PHIaur  1.61803398874989490252573887119069695472717285156250
// --- anything that avoids division overflow: ---
#define SMALL_NUM  0.0000000001

// --- round a to nearest int: ---
#define ROUND(a)    ((a)>0 ? (int)(a+0.5) : -(int)(0.5-a))  

// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declare as: "double my_vec[n];"  (n min 2)
// ***********************************************************************

// --- returns squared length of input vector: ---    
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_SquaredLength(double *, double *); 
    
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_Length(double *, double *); 

// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Add(double *, double *, double *);

// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Sub(double *, double *, double *);

// --- return the vector perpendicular to the input vector vers_1: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void V2D_MakePerpendicular(double *, double *);

// --- normalizes the input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void NormalizeVers2D(double *, double *);

// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* declare vec as: double my_vec[n]; (n min 3) 
// ******* declare matrix as: double my_matrix[16];
// *******  [0]  [4]  [ 8]
// *******  [1]  [5]  [ 9]
// *******  [2]  [6]  [10] will be used.
// *****************************************************************

// --- Vector (or XYZ point) * matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XMatr33(double *, double *, double *);

// --- Vector (or xyz point) * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XtraspMatr33(double *, double *, double *);

// --- matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrXMatr33(double *, double *, double *);

// --- matrix * transposed matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrXtraspMatr33(double *, double *, double *);

// --- transposed matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void TraspMatrXMatr33(double *, double *, double *);

// --- Matrix addition: ---
// Matrix addition is commutative.
// Matrix addition is associative.
// This means that ( a + b ) + c = a + ( b + c ).
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrAdd33(double *, double *, double *);

// --- Matrix subtraction: ---
// Matrix subtraction is NOT commutative.
// This means that you can't change the order when doing a - b.
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrSubtract33(double *, double *, double *);

// --- transposition: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void trasp_mat33(double *, double *);

// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void scalemul33_ret3(double *, double *, double *);

// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr [0],[4],[8]   | par 1: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]   |                     [1],[5],[9]
//                     [2],[6],[10]  |                     [2],[6],[10]
// par 2: InpVal, var
void scalemul33_ret1(double *, double);

// --- determinant: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
double determinant33(double *);

// --- matrix inversion: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void InvMatr33(double *, double *);

// --- normalize a vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, vect [0],[1],[2]
void NormalizeVers3D(double *, double *);

// ***************************************************
// *** 3 routines for matrix orthonormalization:
// *** double matr[16], orientation part:
// *** [0],[4],[8]
// *** [1],[5],[9]
// *** [2],[6],[10]
// *** attention to speed!
// *** benchmarks (1000000 calls):
// *** matr_reortho33     elapsed: 1.618701 --- "best fit"     orthonormalization ---
// *** matr_orthonorm33   elapsed: 0.189059 --- "hierarchical" orthonormalization ---
// *** reorthonormalize33 elapsed: 0.076302 --- "casual"       orthonormalization ---

// --- "best fit" orthonormalization: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void matr_reortho33(double *, double *);

// --- "hierarchical" orthonormalization: ---
// --- par. 1, int, is a flag, meaning:
// --- 1=XYZ  2=XZY  3=YZX  4=YXZ  5=ZXY  6=ZYX
// --- (I axs unchanged, II forced to 90 degrees on the plane, III calculated)
// par 1: InpVal, var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void matr_orthonorm33(int, double *, double *);

// --- "casual" orthonormalization (best speed): ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 1: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void reorthonormalize33(double *);

// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_SquaredLength(double *, double *); 

// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_Length(double *, double *); 

// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Add(double *, double *, double *);

// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Sub(double *, double *, double *);

// --- make a linear combination of two vectors and return the result. ---
// --- result = (avect * ascl) + (bvect * bscl) ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, &var
// par 4: InpVal, &var
// par 5: RetVal, vect [0],[1],[2]
void V3D_Combine (double *, double *, double *, double *, double *);

// --- Find vector of the segment from point A (xyz) to point B (xyz): ---
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, point [0],[1],[2]
// par 3: RetVal, vect  [0],[1],[2]
void s2vrs(double *, double *, double *);

// --- angle between 2 vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotang (double *, double *, double *);

// --- like dotang(), but returns angle in radians: ---
void dotang_rad(double *, double *, double *);

// --- cosine of a vector on another vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotprod (double *, double *, double *);

// --- return a vector normal to the plane of the 2 input vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void crossprod(double *, double *, double *);

// --- returns the rotation matrix for an angle around X/Y/Z: ---
// par 1: InpVal, &var
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void MatrRotX(double *, double *);
void MatrRotY(double *, double *);
void MatrRotZ(double *, double *);

// --- rotation of a vector (or a xyz point) around X/Y/Z axs: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void RotVecX(double *, double *, double *);
void RotVecY(double *, double *, double *);
void RotVecZ(double *, double *, double *);

// --- rotation of a matrix around X/Y/Z axs (only orientation part): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void RotMatrX(double *, double *, double *);
void RotMatrY(double *, double *, double *);
void RotMatrZ(double *, double *, double *);

/* If You want to call Matr33Slerp() o QuatSlerp()
   You have to allocate a "MatrixSlerp" struct:     */
typedef struct
  {
  double mat_from[16];
  double mat_to[16];
  double quat_from[4];
  double quat_to[4];
  double percent;
  double quat_r[4];
  double mat_r[16];
  }MatrixSlerp;

// --- linear interpolation between vectors: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: RetVal, vect [0],[1],[2]
void Vec3Lerp(double *, double *, double *, double *);

// --- spherical interpolation between matrices: ---
// par 1: allocate a "MatrixSlerp" struct
void Matr33Slerp(MatrixSlerp *);

// --- spherical interpolation between quaternions: ---
// par 1: allocate a "MatrixSlerp" struct
void QuatSlerp(MatrixSlerp *);

// --- from direct cosines matrix to Euler angles: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2]
void Dcm33ToEul(double *, double *);

// --- from Euler angles to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2]
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void EulToDcm33(double *, double *);

// --- from direct cosines matrix to quaternions: ---
// par 1: InpVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2],[3]  declare: "double miovers[n];" (n min 4)
void Matr33ToQuat(double *, double *);

// --- from quaternions to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2],[3] declare.: "double my_vect[n];" (n min 4)
// par 2: RetVal, matr [0],[4],[8]
//                     [1],[5],[9]
//                     [2],[6],[10]
void QuatToMatr33(double *, double *);

// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4) 
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************

// --- vector * matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XMatr44(double *, double *, double *);

// --- vector * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XtraspMatr44(double *, double *, double *);

// --- matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXMatr44(double *, double *, double *);

// --- matrix * transposed matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXtraspMatr44(double *, double *, double *);

// --- transposed matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void TraspMatrXMatr44(double *, double *, double *);

// --- Matrix addition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrAdd44(double *, double *, double *);

// --- Matrix subtraction: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrSubtract44(double *, double *, double *);

// --- transposition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void trasp_mat44(double *, double *);

// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void scalemul44_ret3(double *, double *, double *);

// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr[16] (column matr)  |  par 1: RetVal, matr[16] (column matr)
// par 2: InpVal, var
void scalemul44_ret1(double *, double);

// --- determinant: ---
// par 1: InpVal, matr[16] (column matr)
double determinant44(double *);

// --- matrix inversion: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void InvMatr44(double *, double *);

// --- normalize a vector (ok also for quaternions): ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: RetVal, vect [0],[1],[2],[3]
void NormalizeVers4(double *, double *);

// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************

// --- direct transformation of a vector: (Vec3XtraspMatr33) ---
//  par 1: ImpVal, vec [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------- cosines ----
//                 matr [0],[4],[8] 
//                      [1],[5],[9] 
//                      [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
#define transf_dir_vec33 Vec3XtraspMatr33

// --- inverse transformation of a vector: (Vec3XMatr33) ---
//  par 1: ImpVal, vec [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------- cosines ----
//                 matr [0],[4],[8] 
//                      [1],[5],[9] 
//                      [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
#define transf_inv_vec33 Vec3XMatr33

// --- direct transformation of a point: (sub, Vec3XtraspMatr33) ---
//  par 1: ImpVal, point [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------ cosines --|-- ori (xyz) --
//                 matr [0],[4],[8] ,    [12]
//                      [1],[5],[9] ,    [13]
//                      [2],[6],[10],    [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_dir_pt331(double *, double *, double *);

// --- inverse transformation of a point: (Vec3XMatr33, add) ---
//  par 1: ImpVal, point [0],[1],[2]
//
//  par 2: InpVal, matrix declared matr[16]
//                 ------ cosines --|-- ori (xyz) --
//                 matr [0],[4],[8] ,    [12]
//                      [1],[5],[9] ,    [13]
//                      [2],[6],[10],    [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_inv_pt331(double *, double *, double *);

// --- direct transformation of a matrix: MatrXtraspMatr33,
//                                           (sub, Vec3XtraspMatr33) ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: InpVal, matr[16]
//  par 3: RetVal, matr[16]
void transf_dir_axs331(double *, double *, double *);

// --- inverse transformation of a matrix: MatrXMatr33,
//                                           (Vec3XMatr33, add) ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: InpVal, matr[16]
//  par 3: RetVal, matr[16]
void transf_inv_axs331(double *, double *, double *);

// --- Invert definition of an axis system: ---
// matrix declared matr[16]
//  ------ cosines --|-- ori (xyz) --
//  matr [0],[4],[8] ,    [12]
//   [1],[5],[9] ,    [13]
//   [2],[6],[10],    [14]
//
//  par 1: ImpVal, matr[16]
//  par 2: RetVal, matr[16]
void inv_axs_def331(double *, double *);

// --- return identity matrix: ---
//  par 1: InpVal, matrix declared matr[16]
//                 matr [0],[4],[8] ,[12]
//                      [1],[5],[9] ,[13]
//                      [2],[6],[10],[14]
//                      [3],[7],[11],[15]
//
//  par 1: RetVal, 1   0   0   0
//      0   1   0   0
//      0   0   1   0
//                 0   0   0   1
void MatrIdentify( double *);

// --- print on console (printf()) a matrix: ---
// --- mode(par1): 1) 3x3,col   2) 3x3,row   3) 4x4,col   4) 4x4,row
// matrix declared matr[16]
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, matr [0],[4],[8] 
//                     [1],[5],[9] 
//                     [2],[6],[10]
// If par 1 = 3 o par 1 = 4  par 2 will be all the matrix[16]
void print_matr(int, double *);

// --- print on console (printf()) a vector (3 o 4 elements)
// --- mode(par1): 1) vect3,col   2) vect3,row   3) vect4,col   4) vect4,row
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, vect [0],[1],[2]
// If par 1 = 3 o par 1 = 4  par 2 will be all vect[4]
void print_vers(int, double *);

// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */

// --- distance between 2 points xy(2D): ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, &var
void dist_pt2pt2(double *, double *, double *);

// --- distance between 2 points xyz(3D): ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dist_pt3pt3(double *, double *, double *);

// --- intersection of 2 circ(2D): 1° circ: x,y center and radius.
// ---                             2° circ: x,y center and radius.
// --- Return in x1,y1 e x2,y2 ---
// par 1: InpVal, &var   |   par 1: RetVal, &var
// par 2: InpVal, &var   |   par 2: RetVal, &var
// par 3: InpVal, var
// par 4: InpVal, &var   |   par 4: RetVal, &var
// par 5: InpVal, &var   |   par 5: RetVal, &var
// par 6: InpVal, var
int c2c (double *, double *, double, double *, double *, double);

// --- triangle angles, if we know sides. ---
// par 1: InpVal, var
// par 2: InpVal, var
// par 3: InpVal, var
// par 4: RetVal, vect [0],[1],[2]
void tri(double, double, double, double *);

// --- 4 points on a sphere, returns center (xyz) and radius: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: InpVal, vect [0],[1],[2]
// par 5: RetVal, vect [0],[1],[2]
// par 6: RetVal, &var
int sfera(double *, double *, double *, double *, double *, double *);

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- distance point - line (3d). ---
// double line[8];
// par 1: InpVal, point [0],[1],[2] 
// par 2: InpVal, line  [0],[1],[2] --- start pt
//                      [4],[5],[6] --- end pt
// par 3: RetVal, var               --- distance
// par 4: RetVal, vect [0],[1],[2]  --- pt on the line
void dist_Point_to_Line( double *, double *, double *, double *);

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// get base of perpendicular from point to a plane
//   Input:  P = a 3D point
//           PL = a plane with point V0 and normal n
//   Output: *B(pt_on_pln) = base point on PL of perpendicular from P
//   Return: the distance from P to the plane PL
//
// double plane[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, plane [0],[1],[2] x,y,z pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2]
// par 4: RetVal, var               distance
void dist_Point_to_Plane( double *, double *, double *, double *);

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- Input:  two 3D lines L1 and L2 --- Return: the shortest distance ---
// double line[8]; // [0],[1],[2] x,y,z start pt    [4],[5],[6] end pt
// par 1: InpVal, line [0],[1],[2] start pt
//                     [4],[5],[6] end pt
// par 2: InpVal, line [0],[1],[2]
//                     [4],[5],[6] end pt
// par 3: RetVal, &var
// par 4: RetVal, &var
// par 5: RetVal, point [0],[1],[2]
// par 6: RetVal, point [0],[1],[2]
//    Return: the shortest distance between L1 and L2,
//            lines parameters in vL1par and vL2par
//            and nearest points in pt_1 and pt_2
double dist3D_Line_to_Line( double *, double *, double *, double *, double *, double *);

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_SegmentPlane(): intersect a segment and a plane
//    Input:  S = a segment, and Pn = a plane = {Point V0; Vector n;}
//    Output: *I0 = the intersect point (when it exists)
//    Return: 0 = disjoint (no intersection)
//            1 = intersection in the unique point *I0
//            2 = the segment lies in the plane
//
// double segm[8]; double plan[8]; double pnt[4]; int *retval;
// par 1: InpVal,  line [0],[1],[2] start pt
//                      [4],[5],[6] end pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2] inters. point
// par 4: RetVal, &var              return flag
void inters_ln_pln( double *, double *, double *, int *);

// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_2Planes(): the 3D intersect of two planes
//    Input:  two planes Pn1 and Pn2
//    Output: *L = the intersection line (when it exists)
//    Return: 0 = disjoint (no intersection)
//            1 = the two planes coincide
//            2 = intersection in the unique line *L
// par 1: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
//                      [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, line  [0],[1],[2] start pt
//                      [4],[5],[6] end pt
// par 4: RetVal, &var              return flag
void inters_pln_pln(double *, double *, double *, int *);

// --- Geometric center of gravity of a triangle, knowing the vertices: ---
// par 1: InpVal, pt [0],[1],[2]
// par 2: InpVal, pt [0],[1],[2]
// par 3: RetVal, pt [0],[1],[2]
// par 4: RetVal, pt [0],[1],[2]
void tri_centroid(double *, double *, double *, double *);

// *****************************************************************
// *****************************************************************