# Thread: Any math code out there using column matrices?

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.

2. 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
•