Part of the Khronos Group
OpenGL.org

The Industry's Foundation for High Performance Graphics

from games to virtual reality, mobile phones to supercomputers

Results 1 to 2 of 2

Thread: Any math code out there using column matrices?

  1. #1
    Newbie Newbie
    Join Date
    Nov 2012
    Posts
    1

    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.
    Last edited by f35f22fan; 11-23-2012 at 03:16 PM.

  2. #2
    Junior Member Newbie rogxx's Avatar
    Join Date
    Oct 2010
    Posts
    9
    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:

    Code :
    /**********************************************
     ***** 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("\nGimball 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\n");
        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\n");
          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\n");
          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\n");
          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)\n");
          printf(" %.10f,      %.10f,      %.10f\n", mat[0], mat[4], mat[8]);
          printf(" %.10f,      %.10f,      %.10f\n", mat[1], mat[5], mat[9]);
          printf(" %.10f,      %.10f,      %.10f\n", mat[2], mat[6], mat[10]);
          break;
          }
        case 2:   // 3x3,row
          {
          printf("(row vers)\n");
          printf(" %.10f,      %.10f,      %.10f\n", mat[0], mat[1], mat[2]);
          printf(" %.10f,      %.10f,      %.10f\n", mat[4], mat[5], mat[6]);
          printf(" %.10f,      %.10f,      %.10f\n", mat[8], mat[9], mat[10]);
          break;
          }
        case 3:   // 4x4,col
          {
          printf("(column vers)\n");
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[0], mat[4], mat[8] , mat[12]);
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[1], mat[5], mat[9] , mat[13]);
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[2], mat[6], mat[10], mat[14]);
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[3], mat[7], mat[11], mat[15]);
          break;
          }
        case 4:   // 4x4,row
          {
          printf("(row vers)\n");
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[0] , mat[1] , mat[2] , mat[3] );
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[4] , mat[5] , mat[6] , mat[7] );
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", mat[8] , mat[9] , mat[10], mat[11]);
          printf(" %.10f,      %.10f,      %.10f,      %.10f\n", 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\n %.10f\n %.10f\n\n", vers[0], vers[1], vers[2]);
          break;
          }
        case 2:   // 3,row
          {
          printf(" %.10f,    %.10f,    %.10f\n\n", vers[0], vers[1], vers[2]);
          break;
          }
        case 3:   // 4,col
          {
          printf(" %.10f\n %.10f\n %.10f\n %.10f\n\n", vers[0], vers[1], vers[2], vers[3]);
          break;
          }
        case 4:   // 4,row
          {
          printf(" %.10f,    %.10f,    %.10f,    %.10f\n\n", 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\n",m11,m12,m13,m14,m15);
      if (m11 == 0) {
         fprintf(stderr,"The points don't define a sphere!\n");
         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\n", 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:

    Code :
    /***********************************************
     ***** 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 *);
     
    // *****************************************************************
    // *****************************************************************

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •