May be this is useful for You.
There are 2 files (.c and .h) where I have collected some routines to play with vectors and matrices in the last years.
All matrices are 4x4 column-major. When You want to call this routines, You have to declare matrices this way:
my_matrix[16];
The quick and dirty way to use it in Your program:
#include “matrixe.c”
But You can do an object file, or a shared library,
or copy and paste the routine(s) You need in Your file.
I’m not a professional programmer, and all here is very simple “plain” C.
You just have to know a bit about pointers, then You can do what You want.
Here are the 2 files: matrixe.c and matrixe.h
matrixe.c:
/**********************************************
***** matrixe.c - Giovanni Rosso *****
***** rogx@libero.it *****
***** v 0.67 20121118 *****
**********************************************
(SORRY FOR MY ENGLISH!)
OpenGl standards: Column direction cosines:
axs X axs Y axs Z OriginiXYZ
--------------------------------------
| cosX cosX cosX X |
M = | cosY cosY cosY Y |
| cosZ cosZ cosZ Z |
| |
| 0 0 0 1 |
--------------------------------------
Serialized matrix (by columns)
The matrix uses the following positions:
--------------------------------------
| mat[0] mat[4] mat[ 8] mat[12] |
M = | mat[1] mat[5] mat[ 9] mat[13] |
| mat[2] mat[6] mat[10] mat[14] |
| |
| mat[3] mat[7] mat[11] mat[15] |
--------------------------------------
*************************************************************
Projection transformations(gluPerspective, glOrtho,
glMatrixMode(GL_PROJECTION)) are left handed.
Everything else is right handed, including vertexes
(glMatrixMode(GL_MODELVIEW)).
************************************************************
Rem: to call the subroutines that work with matrices declare
all matrices this way: double my_matrix[16];
If in the name of the sub You find "33" the operations will
use only the orientation part, ie:
mat[0] mat[4] mat[ 8]
mat[1] mat[5] mat[ 9]
mat[2] mat[6] mat[10]
// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];" (n min 2)
// ***********************************************************************
// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3)
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0] [4] [ 8]
// ******* [1] [5] [ 9]
// ******* [2] [6] [10]
// *****************************************************************
// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4)
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************
// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************
// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */
#include "matrixe.h"
static double coef[16] = /* From mathematica */
{ 1.0, -1.0/2.0, 3.0/8.0, -5.0/16.0, 35.0/128.0, -63.0/256.0,
231.0/1024.0, -429.0/2048.0, 6435.0/32768.0, -12155.0/65536.0 };
static const double matrix_id[16]=
{1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0};
// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];" (n min 2)
// ***********************************************************************
// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_SquaredLength(double *vers, double *VSqLen)
{
*VSqLen = ((vers[0] * vers[0])+(vers[1] * vers[1]));
}
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_Length(double *vers, double *VLen)
{
*VLen = sqrt(((vers[0] * vers[0])+(vers[1] * vers[1])));
}
// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Add(double *vers_1, double *vers_2, double *vers_r)
{
vers_r[0] = vers_1[0] + vers_2[0];
vers_r[1] = vers_1[1] + vers_2[1];
}
// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Sub(double *vers_1, double *vers_2, double *vers_r)
{
vers_r[0] = vers_1[0] - vers_2[0];
vers_r[1] = vers_1[1] - vers_2[1];
}
// --- return the vector perpendicular to the input vector vers_1: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void V2D_MakePerpendicular(double *vers_1, double *vers_r)
{
vers_r[0] = -(vers_1[1]);
vers_r[1] = vers_1[0];
}
// --- normalizes the input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void NormalizeVers2D(double *vec_1, double *vec_r)
{
double k;
k = 1 / sqrt(vec_1[0] * vec_1[0] + vec_1[1] * vec_1[1] );
vec_r[0]= vec_1[0] * k;
vec_r[1]= vec_1[1] * k;
}// *** End NormalizeVers2D() ***
// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3)
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0] [4] [ 8]
// ******* [1] [5] [ 9]
// ******* [2] [6] [10]
// *****************************************************************
// --- Vector (or XYZ point) * matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XMatr33(double *vec_1, double *mat_1, double *vec_r)
{
vec_r[0]=vec_1[0] * mat_1[0] + vec_1[1] * mat_1[4] + vec_1[2] * mat_1[8] ;
vec_r[1]=vec_1[0] * mat_1[1] + vec_1[1] * mat_1[5] + vec_1[2] * mat_1[9] ;
vec_r[2]=vec_1[0] * mat_1[2] + vec_1[1] * mat_1[6] + vec_1[2] * mat_1[10];
}// *** End Vec3XMatr33() ***
// ***************************************************
// --- Vector (or xyz point) * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XtraspMatr33(double *vec_1, double *mat_1, double *vec_r)
{
vec_r[0]=vec_1[0] * mat_1[0] + vec_1[1] * mat_1[1] + vec_1[2] * mat_1[2] ;
vec_r[1]=vec_1[0] * mat_1[4] + vec_1[1] * mat_1[5] + vec_1[2] * mat_1[6] ;
vec_r[2]=vec_1[0] * mat_1[8] + vec_1[1] * mat_1[9] + vec_1[2] * mat_1[10];
}// *** End Vec3XtraspMatr33() ***
// ***************************************************
// --- matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrXMatr33(double *mat_1, double *mat_2, double *mat_r)
{
mat_r[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[4] + mat_1[2] *mat_2[8] ;
mat_r[1] = mat_1[0]*mat_2[1] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[9] ;
mat_r[2] = mat_1[0]*mat_2[2] + mat_1[1]*mat_2[6] + mat_1[2] *mat_2[10];
mat_r[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[6] *mat_2[8] ;
mat_r[5] = mat_1[4]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[9] ;
mat_r[6] = mat_1[4]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[6] *mat_2[10];
mat_r[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[4] + mat_1[10]*mat_2[8] ;
mat_r[9] = mat_1[8]*mat_2[1] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[9] ;
mat_r[10]= mat_1[8]*mat_2[2] + mat_1[9]*mat_2[6] + mat_1[10]*mat_2[10];
}// *** End MatrXMatr33() ***
// ***************************************************
// --- matrix * transposed matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrXtraspMatr33(double *mat_1, double *mat_2, double *mat_r)
{
mat_r[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[1] + mat_1[2] *mat_2[2] ;
mat_r[1] = mat_1[0]*mat_2[4] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[6] ;
mat_r[2] = mat_1[0]*mat_2[8] + mat_1[1]*mat_2[9] + mat_1[2] *mat_2[10];
mat_r[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[1] + mat_1[6] *mat_2[2] ;
mat_r[5] = mat_1[4]*mat_2[4] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[6] ;
mat_r[6] = mat_1[4]*mat_2[8] + mat_1[5]*mat_2[9] + mat_1[6] *mat_2[10];
mat_r[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[1] + mat_1[10]*mat_2[2] ;
mat_r[9] = mat_1[8]*mat_2[4] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[6] ;
mat_r[10]= mat_1[8]*mat_2[8] + mat_1[9]*mat_2[9] + mat_1[10]*mat_2[10];
}// *** End MatrXtraspMatr33() ***
// ***************************************************
// --- transposed matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void TraspMatrXMatr33(double *mat_1, double *mat_2, double *mat_r)
{
mat_r[0] = mat_1[0]*mat_2[0] + mat_1[4]*mat_2[4] + mat_1[8] *mat_2[8] ;
mat_r[1] = mat_1[0]*mat_2[1] + mat_1[4]*mat_2[5] + mat_1[8] *mat_2[9] ;
mat_r[2] = mat_1[0]*mat_2[2] + mat_1[4]*mat_2[6] + mat_1[8] *mat_2[10];
mat_r[4] = mat_1[1]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[9] *mat_2[8] ;
mat_r[5] = mat_1[1]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[9] *mat_2[9] ;
mat_r[6] = mat_1[1]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[9] *mat_2[10];
mat_r[8] = mat_1[2]*mat_2[0] + mat_1[6]*mat_2[4] + mat_1[10]*mat_2[8] ;
mat_r[9] = mat_1[2]*mat_2[1] + mat_1[6]*mat_2[5] + mat_1[10]*mat_2[9] ;
mat_r[10]= mat_1[2]*mat_2[2] + mat_1[6]*mat_2[6] + mat_1[10]*mat_2[10];
}// *** End TraspMatrXMatr33() ***
// ***************************************************
// --- Matrix addition: ---
// Matrix addition is commutative.
// Matrix addition is associative.
// This means that ( a + b ) + c = a + ( b + c ).
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrAdd33(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0] + mat_2[0];
mat_ret[1] = mat_1[1] + mat_2[1];
mat_ret[2] = mat_1[2] + mat_2[2];
mat_ret[4] = mat_1[4] + mat_2[4];
mat_ret[5] = mat_1[5] + mat_2[5];
mat_ret[6] = mat_1[6] + mat_2[6];
mat_ret[8] = mat_1[8] + mat_2[8];
mat_ret[9] = mat_1[9] + mat_2[9];
mat_ret[10]= mat_1[10] + mat_2[10];
}// *** End MatrAdd33() ***
// ***************************************************
// --- Matrix subtraction: ---
// Matrix subtraction is NOT commutative.
// This means that you can't change the order when doing a - b.
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrSubtract33(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0] - mat_2[0];
mat_ret[1] = mat_1[1] - mat_2[1];
mat_ret[2] = mat_1[2] - mat_2[2];
mat_ret[4] = mat_1[4] - mat_2[4];
mat_ret[5] = mat_1[5] - mat_2[5];
mat_ret[6] = mat_1[6] - mat_2[6];
mat_ret[8] = mat_1[8] - mat_2[8];
mat_ret[9] = mat_1[9] - mat_2[9];
mat_ret[10]= mat_1[10] - mat_2[10];
}// *** End MatrSubtract33() ***
// ***************************************************
// --- transposition: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void trasp_mat33(double *mat_1, double *mat_r)
{
mat_r[0]=mat_1[0]; mat_r[4]=mat_1[1]; mat_r[8] =mat_1[2];
mat_r[1]=mat_1[4]; mat_r[5]=mat_1[5]; mat_r[9] =mat_1[6];
mat_r[2]=mat_1[8]; mat_r[6]=mat_1[9]; mat_r[10]=mat_1[10];
}// *** End trasp_mat33() ***
// ***************************************************
// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void scalemul33_ret3(double *scal, double *mat, double *mat_rr)
{
mat_rr[0] = mat[0] * *scal; mat_rr[4] = mat[4] * *scal; mat_rr[8] = mat[8] * *scal;
mat_rr[1] = mat[1] * *scal; mat_rr[5] = mat[5] * *scal; mat_rr[9] = mat[9] * *scal;
mat_rr[2] = mat[2] * *scal; mat_rr[6] = mat[6] * *scal; mat_rr[10]= mat[10]* *scal;
}// *** End scalemul33_ret3() ***
// ***************************************************
// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr [0],[4],[8] | par 1: RetVal, matr [0],[4],[8]
// [1],[5],[9] | [1],[5],[9]
// [2],[6],[10] | [2],[6],[10]
// par 2: InpVal, var
void scalemul33_ret1(double *mat, double scale)
{
mat[ 0] *= scale; mat[ 4] *= scale; mat[ 8] *= scale;
mat[ 1] *= scale; mat[ 5] *= scale; mat[ 9] *= scale;
mat[ 2] *= scale; mat[ 6] *= scale; mat[10] *= scale;
}// *** End scalemul33_ret1() ***
// --- determinant: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
double determinant33(double *mat)
{
double value;
value = mat[2]*mat[4]*mat[ 9]-mat[0]*mat[6]*mat[ 9] -
mat[2]*mat[5]*mat[ 8]+mat[1]*mat[6]*mat[ 8] -
mat[1]*mat[4]*mat[10]+mat[0]*mat[5]*mat[10] ;
return(value);
}// *** End determinant33() ***
// --- matrix inversion: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void InvMatr33(double *mat, double *mat_ret)
{
double det = determinant33(mat);
mat_ret[0] = mat[ 5]*mat[10] - mat[ 6]*mat[ 9];
mat_ret[1] = mat[ 2]*mat[ 9] - mat[ 1]*mat[10];
mat_ret[2] = mat[ 1]*mat[ 6] - mat[ 2]*mat[ 5];
mat_ret[4] = mat[ 6]*mat[ 8] - mat[ 4]*mat[10];
mat_ret[5] = mat[ 0]*mat[10] - mat[ 2]*mat[ 8];
mat_ret[6] = mat[ 2]*mat[ 4] - mat[ 0]*mat[ 6];
mat_ret[8] = mat[ 4]*mat[ 9] - mat[ 5]*mat[ 8];
mat_ret[9] = mat[ 1]*mat[ 8] - mat[ 0]*mat[ 9];
mat_ret[10]= mat[ 0]*mat[ 5] - mat[ 1]*mat[ 4];
scalemul33_ret1(mat_ret, 1/det);
}// *** End InvMatr33() ***
// *****************************************************
// --- normalize a vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, vect [0],[1],[2]
void NormalizeVers3D(double *vec_1, double *vec_r)
{
double k;
k = 1 / sqrt(vec_1[0] * vec_1[0] + vec_1[1] * vec_1[1] + vec_1[2] * vec_1[2] );
vec_r[0]= vec_1[0] * k;
vec_r[1]= vec_1[1] * k;
vec_r[2]= vec_1[2] * k;
}// *** End NormalizeVers3() ***
// ***************************************************
// ***************************************************
// *** 3 routines for matrix orthonormalization:
// *** double matr[16], orientation part:
// *** [0],[4],[8]
// *** [1],[5],[9]
// *** [2],[6],[10]
// *** attention to speed!
// *** benchmarks (1000000 calls):
// *** matr_reortho33 elapsed: 1.618701 --- "best fit" orthonormalization ---
// *** matr_orthonorm33 elapsed: 0.189059 --- "hierarchical" orthonormalization ---
// *** reorthonormalize33 elapsed: 0.076302 --- "casual" orthonormalization ---
// --- "best fit" orthonormalization: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void matr_reortho33(double *R, double *mat_ret)
{
/* Eric Raible from "Graphics Gems", Academic Press, 1990
*
* Reorthogonalize matrix R - that is find an orthogonal matrix that is
* "close" to R by computing an approximation to the orthogonal matrix
*
* T -1/2
* RC = R(R R)
* T -1
* [RC is orthogonal because (RC) = (CR) ]
* -1/2
* To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
* (where x = C - I) about x=0.
* This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
*/
double I[16], Temp[16], X[16], X_power[16], Sum[16];
int power;
TraspMatrXMatr33(R, R, Temp); /* RtR */
MatrIdentify(I);
MatrSubtract33(Temp, I, X); /* RtR - I */
MatrIdentify(X_power); /* X^0 */
MatrIdentify(Sum); /* coef[0] * X^0 */
for (power = 1; power < 10; power++)
{
MatrXMatr33(X_power, X, Temp);
X_power[0] = Temp[0]; X_power[4] = Temp[4]; X_power[8] = Temp[8];
X_power[1] = Temp[1]; X_power[5] = Temp[5]; X_power[9] = Temp[9];
X_power[2] = Temp[2]; X_power[6] = Temp[6]; X_power[10]= Temp[10];
scalemul33_ret3(&coef[power], X_power, Temp);
MatrAdd33(Sum, Temp, Sum);
}
MatrXMatr33(R, Sum, Temp);
NormalizeVers3D(Temp, mat_ret);
NormalizeVers3D(&Temp[4], &mat_ret[4]);
NormalizeVers3D(&Temp[8], &mat_ret[8]);
}// *** End matr_reortho33() ***
// ***************************************************
// --- "hierarchical" orthonormalization: ---
// --- par. 1, int, is a flag, meaning:
// --- 1=XYZ 2=XZY 3=YZX 4=YXZ 5=ZXY 6=ZYX
// --- (I axs unchanged, II forced to 90 degrees on the plane, III calculated)
// par 1: InpVal, var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void matr_orthonorm33(int xyz123, double *mat_1, double *mat_r)
{
/* 1 = XYZ (123) dx X<=X Y<=Y Z<=Z. Ret: X<=X Y<=Y Z<=Z.
2 = XZY (132) sx X<=X Y<=Z Z<=Y. Ret: X<=X Y<=-Z Z<=Y.
3 = YZX (231) dx X<=Y Y<=Z Z<=X. Ret: X<=Z Y<=X Z<=Y.
4 = YXZ (213) sx X<=Y Y<=X Z<=Z. Ret: X<=Y Y<=X Z<=-Z.
5 = ZXY (312) dx X<=Z Y<=X Z<=Y. Ret: X<=Y Y<=Z Z<=X.
6 = ZYX (321) sx X<=Z Y<=Y Z<=X. Ret: X<=-Z Y<=Y Z<=X. */
double mswp[16];
double x[3], y[3], z[3];
double x_r[3],y_r[3],z_r[3];
mswp[0]=0; mswp[4]=0; mswp[1]=0; mswp[5]=0; mswp[2]=0; mswp[6]=0;
switch(xyz123)
{
case 1:
{
mswp[0]=mat_1[0] ; mswp[4]=mat_1[4] ; mswp[8] =mat_1[8] ;
mswp[1]=mat_1[1] ; mswp[5]=mat_1[5] ; mswp[9] =mat_1[9] ;
mswp[2]=mat_1[2] ; mswp[6]=mat_1[6] ; mswp[10]=mat_1[10];
break;
}
case 2:
{/* X <= X Y <= Z Z <= Y. Rit: X<=X Y<=-Z Z<=Y. */
mswp[0]=mat_1[0] ; mswp[4]=mat_1[8] ; mswp[8] =mat_1[4] ;
mswp[1]=mat_1[1] ; mswp[5]=mat_1[9] ; mswp[9] =mat_1[5] ;
mswp[2]=mat_1[2] ; mswp[6]=mat_1[10]; mswp[10]=mat_1[6] ;
break;
}
case 3:
{/* X <= Y Y <= Z Z <= X. Rit: X<=Z Y<=X Z<=Y. */
mswp[0]=mat_1[4] ; mswp[4]=mat_1[8] ; mswp[8] =mat_1[0] ;
mswp[1]=mat_1[5] ; mswp[5]=mat_1[9] ; mswp[9] =mat_1[1] ;
mswp[2]=mat_1[6] ; mswp[6]=mat_1[10]; mswp[10]=mat_1[2] ;
break;
}
case 4:
{/* X <= Y Y <= X Z <= Z. Rit: X<=Y Y<=X Z<=-Z. */
mswp[0]=mat_1[4] ; mswp[4]=mat_1[0] ; mswp[8] =mat_1[8] ;
mswp[1]=mat_1[5] ; mswp[5]=mat_1[1] ; mswp[9] =mat_1[9] ;
mswp[2]=mat_1[6] ; mswp[6]=mat_1[2] ; mswp[10]=mat_1[10];
break;
}
case 5:
{/* X <= Z Y <= X Z <= Y. Rit: X<=Y Y<=Z Z<=X */
mswp[0]=mat_1[8] ; mswp[4]=mat_1[0] ; mswp[8] =mat_1[4];
mswp[1]=mat_1[9] ; mswp[5]=mat_1[1] ; mswp[9] =mat_1[5];
mswp[2]=mat_1[10]; mswp[6]=mat_1[2] ; mswp[10]=mat_1[6];
break;
}
case 6:
{/* X <= Z Y <= Y Z <= X. Rit: X<=-Z Y<=Y Z<=X. */
mswp[0]=mat_1[8] ; mswp[4]=mat_1[4] ; mswp[8] =mat_1[0] ;
mswp[1]=mat_1[9] ; mswp[5]=mat_1[5] ; mswp[9] =mat_1[1] ;
mswp[2]=mat_1[10]; mswp[6]=mat_1[6] ; mswp[10]=mat_1[2] ;
break;
}
}
x[0]= mswp[0];
x[1]= mswp[1];
x[2]= mswp[2];
y[0]= mswp[4];
y[1]= mswp[5];
y[2]= mswp[6];
z[0]= 0.0;
z[1]= 0.0;
z[2]= 1.0;
NormalizeVers3D(x, x_r);
crossprod(x_r, y, z);
NormalizeVers3D(z, z_r);
crossprod(z_r, x_r, y);
NormalizeVers3D(y, y_r);
mswp[0]= x_r[0];
mswp[1]= x_r[1];
mswp[2]= x_r[2];
mswp[4]= y_r[0];
mswp[5]= y_r[1];
mswp[6]= y_r[2];
mswp[8]= z_r[0];
mswp[9]= z_r[1];
mswp[10]=z_r[2];
switch(xyz123)
{
case 1:
{
mat_r[0]=mswp[0] ; mat_r[4]=mswp[4] ; mat_r[8] =mswp[8] ;
mat_r[1]=mswp[1] ; mat_r[5]=mswp[5] ; mat_r[9] =mswp[9] ;
mat_r[2]=mswp[2] ; mat_r[6]=mswp[6] ; mat_r[10]=mswp[10];
break;
}
case 2:
{/* X <= X Y <= Z Z <= Y.
Rit: X <= X Y <= -Z Z <= Y. */
mat_r[0]=mswp[0] ; mat_r[4]=mswp[8] * -1.0; mat_r[8] =mswp[4] ;
mat_r[1]=mswp[1] ; mat_r[5]=mswp[9] * -1.0; mat_r[9] =mswp[5] ;
mat_r[2]=mswp[2] ; mat_r[6]=mswp[10] * -1.0; mat_r[10]=mswp[6] ;
break;
}
case 3:
{/* X <= Y Y <= Z Z <= X.
Rit: X <= Z Y <= X Z <= Y. */
mat_r[0]=mswp[8] ; mat_r[4]=mswp[0] ; mat_r[8] =mswp[4] ;
mat_r[1]=mswp[9] ; mat_r[5]=mswp[1] ; mat_r[9] =mswp[5] ;
mat_r[2]=mswp[10]; mat_r[6]=mswp[2] ; mat_r[10]=mswp[6] ;
break;
}
case 4:
{/* X <= Y Y <= X Z <= Z.
Rit: X <= Y Y <= X Z <= -Z. */
mat_r[0]=mswp[4] ; mat_r[4]=mswp[0] ; mat_r[8] =mswp[8] * -1.0;
mat_r[1]=mswp[5] ; mat_r[5]=mswp[1] ; mat_r[9] =mswp[9] * -1.0;
mat_r[2]=mswp[6] ; mat_r[6]=mswp[2] ; mat_r[10]=mswp[10] * -1.0;
break;
}
case 5:
{/* X <= Z Y <= X Z <= Y.
Rit: X <= Y Y <= Z Z <= X */
mat_r[0]=mswp[4] ; mat_r[4]=mswp[8] ; mat_r[8] =mswp[0];
mat_r[1]=mswp[5] ; mat_r[5]=mswp[9] ; mat_r[9] =mswp[1];
mat_r[2]=mswp[6] ; mat_r[6]=mswp[10]; mat_r[10]=mswp[2];
break;
}
case 6:
{/* X <= Z Y <= Y Z <= X.
Rit: X <= -Z Y <= Y Z <= X. */
mat_r[0]=mswp[8] * -1.0; mat_r[4]=mswp[4] ; mat_r[8] =mswp[0] ;
mat_r[1]=mswp[9] * -1.0; mat_r[5]=mswp[5] ; mat_r[9] =mswp[1] ;
mat_r[2]=mswp[10]* -1.0; mat_r[6]=mswp[6] ; mat_r[10]=mswp[2];
break;
}
}
}// *** End matr_orthonorm33() ***
// ***************************************************
// --- "casual" orthonormalization (best speed): ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 1: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void reorthonormalize33(double *mtrx_REORTH)
{
static int vec1ndx, vec2ndx, vec3ndx, lessdev;
static double axsang[4];
dotang(mtrx_REORTH, &mtrx_REORTH[4], &axsang[0]);
dotang(&mtrx_REORTH[4], &mtrx_REORTH[8], &axsang[1]);
dotang(&mtrx_REORTH[8], mtrx_REORTH, &axsang[2]);
axsang[0]=fabs(axsang[0] - 90.0);
axsang[1]=fabs(axsang[1] - 90.0);
axsang[2]=fabs(axsang[2] - 90.0);
if((axsang[0]>0.000001) || (axsang[1]>0.000001) || (axsang[2]>0.000001))
{
lessdev=0; // X - Y less deviation
if (axsang[1]<axsang[0])lessdev=1; // Y - Z less deviation
if (axsang[2]<axsang[lessdev])lessdev=2; // Z - X less deviation
switch (lessdev)
{
case 0:
vec1ndx=0; vec2ndx=4; vec3ndx=8;
break;
case 1:
vec1ndx=4; vec2ndx=8; vec3ndx=0;
break;
case 2:
vec1ndx=8; vec2ndx=0; vec3ndx=4;
}
NormalizeVers3D(&mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec1ndx]);
NormalizeVers3D(&mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec2ndx]);
crossprod(&mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec3ndx]);
NormalizeVers3D(&mtrx_REORTH[vec3ndx], &mtrx_REORTH[vec3ndx]);
crossprod(&mtrx_REORTH[vec3ndx], &mtrx_REORTH[vec1ndx], &mtrx_REORTH[vec2ndx]);
NormalizeVers3D(&mtrx_REORTH[vec2ndx], &mtrx_REORTH[vec2ndx]);
}// *** End if((axsang[0]>0.000001) || ... ***
}// *** End reorthonormalize33() ***
// ***************************************************
// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_SquaredLength(double *vers, double *VSqLen)
{
*VSqLen = ((vers[0] * vers[0]) + (vers[1] * vers[1]) + (vers[2] * vers[2]));
}
// ***************************************************
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_Length(double *vers, double *VLen)
{
*VLen = sqrt(((vers[0] * vers[0]) + (vers[1] * vers[1]) + (vers[2] * vers[2])));
}
// ***************************************************
// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Add(double *vers_1, double *vers_2, double *vers_r)
{
vers_r[0] = vers_1[0] + vers_2[0];
vers_r[1] = vers_1[1] + vers_2[1];
vers_r[2] = vers_1[2] + vers_2[2];
}
// ***************************************************
// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Sub(double *vers_1, double *vers_2, double *vers_r)
{
vers_r[0] = vers_1[0] - vers_2[0];
vers_r[1] = vers_1[1] - vers_2[1];
vers_r[2] = vers_1[2] - vers_2[2];
}
// ***************************************************
// --- make a linear combination of two vectors and return the result. ---
// --- result = (avect * ascl) + (bvect * bscl) ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, &var
// par 4: InpVal, &var
// par 5: RetVal, vect [0],[1],[2]
void V3D_Combine (double *vers_1, double *vers_2, double *ascl, double *bscl, double *result)
{
result[0] = (*ascl * vers_1[0]) + (*bscl * vers_2[0]);
result[1] = (*ascl * vers_1[1]) + (*bscl * vers_2[1]);
result[2] = (*ascl * vers_1[2]) + (*bscl * vers_2[2]);
}
// ***************************************************
// --- Find vector of the segment from point A (xyz) to point B (xyz): ---
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, point [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void s2vrs(double pt_A[3], double pt_B[3], double *vers)
{
double dnmt=0;
dnmt=sqrt((pt_B[0]-pt_A[0])*(pt_B[0]-pt_A[0]) +
(pt_B[1]-pt_A[1])*(pt_B[1]-pt_A[1]) +
(pt_B[2]-pt_A[2])*(pt_B[2]-pt_A[2]));
vers[0]=(pt_B[0]-pt_A[0])/dnmt;
vers[1]=(pt_B[1]-pt_A[1])/dnmt;
vers[2]=(pt_B[2]-pt_A[2])/dnmt;
}// *** End s2vrs() ***
// ***************************************************
// --- angle between 2 vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotang (double *vec_1, double *vec_2, double *Ang_r)
{
*Ang_r = TODEGR * acos((vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]));
}// *** End dotang() ***
// ***************************************************
// --- like dotang(), but returns angle in radians: ---
void dotang_rad(double *vec_1, double *vec_2, double *Ang_r)
{
*Ang_r = acos((vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]));
}// *** End dotang_rad() ***
// --- cosine of a vector on another vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotprod (double *vec_1, double *vec_2, double *Ang_r)
{
*Ang_r = (vec_1[0] * vec_2[0]+ vec_1[1] * vec_2[1] + vec_1[2] * vec_2[2]);
}// *** End dotprod() ***
// ***************************************************
// --- return a vector normal to the plane of the 2 input vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void crossprod(double *vec_1, double *vec_2, double *vec_r)
{
vec_r[0]= vec_1[1] * vec_2[2] - vec_1[2] * vec_2[1];
vec_r[1]= vec_1[2] * vec_2[0] - vec_1[0] * vec_2[2];
vec_r[2]= vec_1[0] * vec_2[1] - vec_1[1] * vec_2[0];
}// *** End crossprod() ***
// ***************************************************
// --- returns the rotation matrix for an angle around X/Y/Z: ---
// par 1: InpVal, &var
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrRotX(double *Ang_1, double *mat_rt )
{
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat_rt[0]= 1.0 ; mat_rt[4]= 0.0 ; mat_rt[8] = 0.0 ;
mat_rt[1]= 0.0 ; mat_rt[5]= cos(rdAng); mat_rt[9] =-sin(rdAng) ;
mat_rt[2]= 0.0 ; mat_rt[6]= sin(rdAng); mat_rt[10]= cos(rdAng) ;
}// *** End MatrRotX() ***
void MatrRotY(double *Ang_1, double *mat_rt )
{
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat_rt[0]= cos(rdAng); mat_rt[4]= 0.0 ; mat_rt[8] = sin(rdAng);
mat_rt[1]= 0.0 ; mat_rt[5]= 1.0 ; mat_rt[9] = 0.0 ;
mat_rt[2]= -sin(rdAng); mat_rt[6]= 0.0 ; mat_rt[10]= cos(rdAng);
}// *** End MatrRotY() ***
void MatrRotZ(double *Ang_1, double *mat_rt )
{
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat_rt[0]= cos(rdAng); mat_rt[4]=-sin(rdAng); mat_rt[8] = 0.0 ;
mat_rt[1]= sin(rdAng); mat_rt[5]= cos(rdAng); mat_rt[9] = 0.0 ;
mat_rt[2]= 0.0 ; mat_rt[6]= 0.0 ; mat_rt[10]= 1.0 ;
}// *** End MatrRotZ() ***
// ***************************************************
// --- rotation of a vector (or a xyz point) around X/Y/Z axs: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void RotVecX(double *Ang_1, double *vec_1, double *vec_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= 1.0 ; mat[4]= 0.0 ; mat[8] = 0.0 ;
mat[1]= 0.0 ; mat[5]= cos(rdAng); mat[9] =-sin(rdAng) ;
mat[2]= 0.0 ; mat[6]= sin(rdAng); mat[10]= cos(rdAng) ;
Vec3XMatr33(vec_1, mat, vec_r);
}// *** End RotVecX() ***
void RotVecY(double *Ang_1, double *vec_1, double *vec_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= cos(rdAng); mat[4]= 0.0 ; mat[8] = sin(rdAng);
mat[1]= 0.0 ; mat[5]= 1.0 ; mat[9] = 0.0 ;
mat[2]= -sin(rdAng); mat[6]= 0.0 ; mat[10]= cos(rdAng);
Vec3XMatr33(vec_1, mat, vec_r);
}// *** End RotVecY() ***
void RotVecZ(double *Ang_1, double *vec_1, double *vec_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= cos(rdAng); mat[4]=-sin(rdAng); mat[8] = 0.0 ;
mat[1]= sin(rdAng); mat[5]= cos(rdAng); mat[9] = 0.0 ;
mat[2]= 0.0 ; mat[6]= 0.0 ; mat[10]= 1.0 ;
Vec3XMatr33(vec_1, mat, vec_r);
}// *** End RotVecZ() ***
// ***************************************************
// --- rotation of a matrix around X/Y/Z axs (only orientation part): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void RotMatrX(double *Ang_1, double *mat_1, double *mat_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= 1.0 ; mat[4]= 0.0 ; mat[8] = 0.0 ;
mat[1]= 0.0 ; mat[5]= cos(rdAng); mat[9] =-sin(rdAng) ;
mat[2]= 0.0 ; mat[6]= sin(rdAng); mat[10]= cos(rdAng) ;
MatrXMatr33(mat_1, mat, mat_r);
}// *** End RotMatrX() ***
void RotMatrY(double *Ang_1, double *mat_1, double *mat_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= cos(rdAng); mat[4]= 0.0 ; mat[8] = sin(rdAng);
mat[1]= 0.0 ; mat[5]= 1.0 ; mat[9] = 0.0 ;
mat[2]= -sin(rdAng); mat[6]= 0.0 ; mat[10]= cos(rdAng);
MatrXMatr33(mat_1, mat, mat_r);
}// *** End RotMatrY() ***
void RotMatrZ(double *Ang_1, double *mat_1, double *mat_r)
{
double mat[16];
double rdAng=0.0;
rdAng = *Ang_1 * TORAD;
mat[0]= cos(rdAng); mat[4]=-sin(rdAng); mat[8] = 0.0 ;
mat[1]= sin(rdAng); mat[5]= cos(rdAng); mat[9] = 0.0 ;
mat[2]= 0.0 ; mat[6]= 0.0 ; mat[10]= 1.0 ;
MatrXMatr33(mat_1, mat, mat_r);
}// *** End RotMatrZ() ***
// ***************************************************
// --- linear interpolation between vectors: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: RetVal, vect [0],[1],[2]
void Vec3Lerp(double *lerp, double *vec_1, double *vec_2, double *vec_r)
{
vec_r[0] = vec_1[0] + (*lerp) * (vec_2[0] - vec_1[0]);
vec_r[1] = vec_1[1] + (*lerp) * (vec_2[1] - vec_1[1]);
vec_r[2] = vec_1[2] + (*lerp) * (vec_2[2] - vec_1[2]);
}// *** End Vec3Lerp() ***
// ***************************************************
/* If You want to call Matr33Slerp() o QuatSlerp()
You have to allocate a "MatrixSlerp" struct:
typedef struct
{
double mat_from[16];
double mat_to[16];
double quat_from[4];
double quat_to[4];
double percent;
double quat_r[4];
double mat_r[16];
}MatrixSlerp; */
// --- spherical interpolation between matrices: ---
// par 1: allocate a "MatrixSlerp" struct
void Matr33Slerp(MatrixSlerp *mslerp)
{
Matr33ToQuat(mslerp->mat_from, mslerp->quat_from);
Matr33ToQuat(mslerp->mat_to, mslerp->quat_to);
QuatSlerp(mslerp);
QuatToMatr33(mslerp->quat_r, mslerp->mat_r);
}// *** End Matr33Slerp() ***
// --- spherical interpolation between quaternions: ---
// par 1: allocate a "MatrixSlerp" struct
void QuatSlerp(MatrixSlerp *qslerp)
{
double to1[4];
double omega, cosom, sinom, scale0, scale1;
// calc cosine
cosom = qslerp->quat_from[0] * qslerp->quat_to[0] +
qslerp->quat_from[1] * qslerp->quat_to[1] +
qslerp->quat_from[2] * qslerp->quat_to[2] +
qslerp->quat_from[3] * qslerp->quat_to[3];
// adjust signs (if necessary)
if ( cosom <0.0 )
{
cosom = -cosom;
to1[0] = - qslerp->quat_to[0];
to1[1] = - qslerp->quat_to[1];
to1[2] = - qslerp->quat_to[2];
to1[3] = - qslerp->quat_to[3];
}
else
{
to1[0] = qslerp->quat_to[0];
to1[1] = qslerp->quat_to[1];
to1[2] = qslerp->quat_to[2];
to1[3] = qslerp->quat_to[3];
}
// calculate coefficients
if ( (1.0 - cosom) > 0.00001 ) // if ((1.0 - cosom) > DELTA)
{ // standard case (slerp)
omega = acos(cosom);
sinom = sin(omega);
scale0 = sin((1.0 - qslerp->percent) * omega) / sinom;
scale1 = sin(qslerp->percent * omega) / sinom;
}
else // "from" and "to" quaternions are very close
{ // ... so we can do a linear interpolation
scale0 = 1.0 - qslerp->percent;
scale1 = qslerp->percent;
}
// calculate final values
qslerp->quat_r[0] = scale0 * qslerp->quat_from[0] + scale1 * to1[0];
qslerp->quat_r[1] = scale0 * qslerp->quat_from[1] + scale1 * to1[1];
qslerp->quat_r[2] = scale0 * qslerp->quat_from[2] + scale1 * to1[2];
qslerp->quat_r[3] = scale0 * qslerp->quat_from[3] + scale1 * to1[3];
} // *** End QuatSlerp() ***
// ***************************************************
// --- from direct cosines matrix to Euler angles: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2]
void Dcm33ToEul(double *mat, double *eulang)
{
/* Assuming the angles are in radians.
Kuka robots standard:
|z (seq: first A around Z, second B around Y, then C around X:
| -------------------
|_____ eulang[0] = C - (X)
/ y eulang[1] = B - (Y)
/ eulang[2] = A - (Z)
x ------------------- */
double C=0, tr_x=0, tr_y=0;
eulang[1] = asin( mat[2] ); // *** Calculate Y-axis angle ***
C = cos ( eulang[1]);
eulang[1] *= TODEGR;
if ( fabs( C ) > 0.0005 ) // *** Gimball lock? ***
{
tr_x = mat[10] / C; // *** No, so get X-axis angle ***
tr_y = -mat[6] / C;
eulang[0] = atan2( tr_y, tr_x ) * TODEGR;
tr_x = mat[0] / C; // *** Get Z-axis angle ***
tr_y = -mat[1] / C;
eulang[2] = atan2( tr_y, tr_x ) * TODEGR;
}
else // *** Gimball lock has occurred ***
{
printf("
Gimball lock !!");
eulang[0] = 0; // *** Set X-axis angle to zero ***
tr_x = mat[5]; // *** And calculate Z-axis angle ***
tr_y = mat[4];
eulang[2] = atan2( tr_y, tr_x ) * TODEGR;
}
eulang[2] = -eulang[2]; // Minus sign is
eulang[1] = -eulang[1]; // ok for KUKA - CATIA
eulang[0] = -eulang[0]; // and for CATIA - KUKA
// * return only positive angles in [0,360] *
// * if (eulang[0] < 0) eulang[0] += 360;
// * if (eulang[1] < 0) eulang[1] += 360;
// * if (eulang[2] < 0) eulang[2] += 360;
} // *** End Dcm33ToEul() ***
// ***************************************************
// --- from Euler angles to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void EulToDcm33(double *eulang, double *mat_ret1)
{
// Kuka robots standard: (seq: first A around Z, second B around Y, then C around X:
// eulang[0] = C - (X)
// eulang[1] = B - (Y)
// eulang[2] = A - (Z)
double mat_axs1[16], mat_ret2[16];
double Ang1=0;
mat_axs1[0]= 1.0 ; mat_axs1[4]= 0.0 ; mat_axs1[8] = 0.0 ; mat_axs1[12]= 0.0 ;
mat_axs1[1]= 0.0 ; mat_axs1[5]= 1.0 ; mat_axs1[9] = 0.0 ; mat_axs1[13]= 0.0 ;
mat_axs1[2]= 0.0 ; mat_axs1[6]= 0.0 ; mat_axs1[10]= 1.0 ; mat_axs1[14]= 0.0 ;
mat_axs1[3]= 0.0 ; mat_axs1[7]= 0.0 ; mat_axs1[11]= 0.0 ; mat_axs1[15]= 1.0 ;
Ang1=-eulang[2];
RotMatrZ(&Ang1, mat_axs1, mat_ret2);
Ang1=-eulang[1];
RotMatrY(&Ang1, mat_ret2, mat_ret1);
Ang1=-eulang[0];
RotMatrX(&Ang1, mat_ret1, mat_ret2);
trasp_mat33(mat_ret2, mat_ret1);
} // *** End EulToDcm33() ***
// ***************************************************
// --- from direct cosines matrix to quaternions: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2],[3] declare: "double miovers[n];" (n min 4)
void Matr33ToQuat(double *mat, double *quat)
{
// ******* ABB robot Q1 (the scalar) is quat[3] of this routine! *******
// *** Q2 => quat[0] *** Q3 => quat[1] *** Q4 => quat[2] ***
double trace, s;
trace = 1.0 + mat[0] + mat[5] + mat[10];
if (trace > 0.00000001)
{ //printf("trace > 0.0
");
s = 0.5 / sqrt(trace);
quat[3] = 0.25 / s;
quat[0] = (mat[6] - mat[9]) * s;
quat[1] = (mat[8] - mat[2]) * s;
quat[2] = (mat[1] - mat[4]) * s;
}
else
{
if((mat[0]>mat[5]) && (mat[0]>mat[10]))
{ //printf("trace <= 0.0, col0
");
s = sqrt(1.0 + mat[0] - mat[5] - mat[10]) * 2.0;
quat[0] = s * 0.25;
quat[1] = (mat[1] + mat[4]) / s;
quat[2] = (mat[2] + mat[8]) / s;
quat[3] = (mat[9] - mat[6]) / s;
}
else if(mat[5]>mat[10])
{ //printf("trace <= 0.0, col1
");
s = sqrt(1.0 + mat[5] - mat[0] - mat[10]) * 2.0;
quat[0] = (mat[1] + mat[4]) / s;
quat[1] = s * 0.25;
quat[2] = (mat[6] + mat[9]) / s;
quat[3] = (mat[8] - mat[2]) / s;
}
else
{ //printf("trace <= 0.0, col2
");
s = sqrt(1.0 + mat[10] - mat[0] - mat[5]) * 2.0;
quat[0] = (mat[2] + mat[8]) / s;
quat[1] = (mat[6] + mat[9]) / s;
quat[2] = s * 0.25;
quat[3] = (mat[4] - mat[1]) / s;
}
}
}// *** End Matr33ToQuat() ***
// ***************************************************
// --- from quaternions to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2],[3] declare.: "double my_vect[n];" (n min 4)
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void QuatToMatr33(double *quat, double *mat)
{
double xx = quat[0]*quat[0],
yy = quat[1]*quat[1],
zz = quat[2]*quat[2],
xy = quat[0]*quat[1],
xz = quat[0]*quat[2],
yz = quat[1]*quat[2],
xw = quat[0]*quat[3],
yw = quat[1]*quat[3],
zw = quat[2]*quat[3];
mat[0] = 1.0 - 2.0 * (yy + zz);
mat[4] = 2.0 * (xy - zw);
mat[8] = 2.0 * (xz + yw);
mat[12]= 0.0;
mat[1] = 2.0 * (xy + zw);
mat[5] = 1.0 - 2.0 * (xx + zz);
mat[9] = 2.0 * (yz - xw);
mat[13]= 0.0;
mat[2] = 2.0 * (xz - yw);
mat[6] = 2.0 * (yz + xw);
mat[10]= 1.0 - 2.0 * (xx + yy);
mat[14]= 0.0;
mat[3] = 0.0;
mat[7] = 0.0;
mat[11]= 0.0;
mat[15]= 1.0;
} // *** End QuatToMatr33() ***
// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* declare vec as: double my_vec[n]; (n min 4)
// ******* declare matrix as: double my_matrix[16];
// ******* all matrix will be used.
// *****************************************************************
// --- vector * matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XMatr44(double *vec_i, double *matr, double *vec_r)
{
vec_r[0]=vec_i[0] * matr[0] + vec_i[1] * matr[4] + vec_i[2] * matr[8] + vec_i[3] * matr[12];
vec_r[1]=vec_i[0] * matr[1] + vec_i[1] * matr[5] + vec_i[2] * matr[9] + vec_i[3] * matr[13];
vec_r[2]=vec_i[0] * matr[2] + vec_i[1] * matr[6] + vec_i[2] * matr[10] + vec_i[3] * matr[14];
vec_r[3]=vec_i[0] * matr[3] + vec_i[1] * matr[7] + vec_i[2] * matr[11] + vec_i[3] * matr[15];
}/*** End Vec4XMatr44() ***/
// ***************************************************
// --- vector * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XtraspMatr44(double *vec_i, double *matr, double *vec_r)
{
vec_r[0]=vec_i[0] * matr[0] + vec_i[1] * matr[1] + vec_i[2] * matr[2] + vec_i[3] * matr[3] ;
vec_r[1]=vec_i[0] * matr[4] + vec_i[1] * matr[5] + vec_i[2] * matr[6] + vec_i[3] * matr[7] ;
vec_r[2]=vec_i[0] * matr[8] + vec_i[1] * matr[9] + vec_i[2] * matr[10] + vec_i[3] * matr[11];
vec_r[3]=vec_i[0] * matr[12] + vec_i[1] * matr[13] + vec_i[2] * matr[14] + vec_i[3] * matr[15];
}// *** End Vec4XtraspMatr44() ***
// ***************************************************
// --- matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXMatr44(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[4] + mat_1[2] *mat_2[8] + mat_1[3] *mat_2[12];
mat_ret[1] = mat_1[0]*mat_2[1] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[9] + mat_1[3] *mat_2[13];
mat_ret[2] = mat_1[0]*mat_2[2] + mat_1[1]*mat_2[6] + mat_1[2] *mat_2[10] + mat_1[3] *mat_2[14];
mat_ret[3] = mat_1[0]*mat_2[3] + mat_1[1]*mat_2[7] + mat_1[2] *mat_2[11] + mat_1[3] *mat_2[15];
mat_ret[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[6] *mat_2[8] + mat_1[7] *mat_2[12];
mat_ret[5] = mat_1[4]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[9] + mat_1[7] *mat_2[13];
mat_ret[6] = mat_1[4]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[6] *mat_2[10] + mat_1[7] *mat_2[14];
mat_ret[7] = mat_1[4]*mat_2[3] + mat_1[5]*mat_2[7] + mat_1[6] *mat_2[11] + mat_1[7] *mat_2[15];
mat_ret[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[4] + mat_1[10]*mat_2[8] + mat_1[11]*mat_2[12];
mat_ret[9] = mat_1[8]*mat_2[1] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[9] + mat_1[11]*mat_2[13];
mat_ret[10]= mat_1[8]*mat_2[2] + mat_1[9]*mat_2[6] + mat_1[10]*mat_2[10] + mat_1[11]*mat_2[14];
mat_ret[11]= mat_1[8]*mat_2[3] + mat_1[9]*mat_2[7] + mat_1[10]*mat_2[11] + mat_1[11]*mat_2[15];
mat_ret[12]= mat_1[12]*mat_2[0]+ mat_1[13]*mat_2[4]+ mat_1[14]*mat_2[8] + mat_1[15]*mat_2[12];
mat_ret[13]= mat_1[12]*mat_2[1]+ mat_1[13]*mat_2[5]+ mat_1[14]*mat_2[9] + mat_1[15]*mat_2[13];
mat_ret[14]= mat_1[12]*mat_2[2]+ mat_1[13]*mat_2[6]+ mat_1[14]*mat_2[10] + mat_1[15]*mat_2[14];
mat_ret[15]= mat_1[12]*mat_2[3]+ mat_1[13]*mat_2[7]+ mat_1[14]*mat_2[11] + mat_1[15]*mat_2[15];
}// *** End MatrXMatr44() ***
// ***************************************************
// --- matrix * transposed matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXtraspMatr44(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[1] + mat_1[2] *mat_2[2] + mat_1[3] *mat_2[3] ;
mat_ret[1] = mat_1[0]*mat_2[4] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[6] + mat_1[3] *mat_2[7] ;
mat_ret[2] = mat_1[0]*mat_2[8] + mat_1[1]*mat_2[9] + mat_1[2] *mat_2[10]+ mat_1[3] *mat_2[11];
mat_ret[3] = mat_1[0]*mat_2[12]+ mat_1[1]*mat_2[13] + mat_1[2] *mat_2[14]+ mat_1[3] *mat_2[15];
mat_ret[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[1] + mat_1[6] *mat_2[2] + mat_1[7] *mat_2[3] ;
mat_ret[5] = mat_1[4]*mat_2[4] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[6] + mat_1[7] *mat_2[7] ;
mat_ret[6] = mat_1[4]*mat_2[8] + mat_1[5]*mat_2[9] + mat_1[6] *mat_2[10]+ mat_1[7] *mat_2[11];
mat_ret[7] = mat_1[4]*mat_2[12]+ mat_1[5]*mat_2[13] + mat_1[6] *mat_2[14]+ mat_1[7] *mat_2[15];
mat_ret[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[1] + mat_1[10]*mat_2[2] + mat_1[11]*mat_2[3] ;
mat_ret[9] = mat_1[8]*mat_2[4] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[6] + mat_1[11]*mat_2[7] ;
mat_ret[10]= mat_1[8]*mat_2[8] + mat_1[9]*mat_2[9] + mat_1[10]*mat_2[10]+ mat_1[11]*mat_2[11];
mat_ret[11]= mat_1[8]*mat_2[12]+ mat_1[9]*mat_2[13] + mat_1[10]*mat_2[14]+ mat_1[11]*mat_2[15];
mat_ret[12]=mat_1[12]*mat_2[0] + mat_1[13]*mat_2[1] + mat_1[14]*mat_2[2] + mat_1[15]*mat_2[3] ;
mat_ret[13]=mat_1[12]*mat_2[4] + mat_1[13]*mat_2[5] + mat_1[14]*mat_2[6] + mat_1[15]*mat_2[7] ;
mat_ret[14]=mat_1[12]*mat_2[8] + mat_1[13]*mat_2[9] + mat_1[14]*mat_2[10]+ mat_1[15]*mat_2[11];
mat_ret[15]=mat_1[12]*mat_2[12]+ mat_1[13]*mat_2[13]+ mat_1[14]*mat_2[14]+ mat_1[15]*mat_2[15];
}// *** End MatrXtraspMatr44() ***
// ***************************************************
// --- transposed matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void TraspMatrXMatr44(double *mat_1, double *mat_2, double *mat_r)
{
mat_r[0] = mat_1[0]*mat_2[0] + mat_1[4]*mat_2[4] + mat_1[8] *mat_2[8] + mat_1[12]*mat_2[12];
mat_r[1] = mat_1[0]*mat_2[1] + mat_1[4]*mat_2[5] + mat_1[8] *mat_2[9] + mat_1[12]*mat_2[13];
mat_r[2] = mat_1[0]*mat_2[2] + mat_1[4]*mat_2[6] + mat_1[8] *mat_2[10] + mat_1[12]*mat_2[14];
mat_r[3] = mat_1[0]*mat_2[3] + mat_1[4]*mat_2[7] + mat_1[8] *mat_2[11] + mat_1[12]*mat_2[15];
mat_r[4] = mat_1[1]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[9] *mat_2[8] + mat_1[13]*mat_2[12];
mat_r[5] = mat_1[1]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[9] *mat_2[9] + mat_1[13]*mat_2[13];
mat_r[6] = mat_1[1]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[9] *mat_2[10] + mat_1[13]*mat_2[14];
mat_r[7] = mat_1[1]*mat_2[3] + mat_1[5]*mat_2[7] + mat_1[9] *mat_2[11] + mat_1[13]*mat_2[15];
mat_r[8] = mat_1[2]*mat_2[0] + mat_1[6]*mat_2[4] + mat_1[10]*mat_2[8] + mat_1[14]*mat_2[12];
mat_r[9] = mat_1[2]*mat_2[1] + mat_1[6]*mat_2[5] + mat_1[10]*mat_2[9] + mat_1[14]*mat_2[13];
mat_r[10]= mat_1[2]*mat_2[2] + mat_1[6]*mat_2[6] + mat_1[10]*mat_2[10] + mat_1[14]*mat_2[14];
mat_r[11]= mat_1[2]*mat_2[3] + mat_1[6]*mat_2[7] + mat_1[10]*mat_2[11] + mat_1[14]*mat_2[15];
mat_r[12]= mat_1[3]*mat_2[0] + mat_1[7]*mat_2[4] + mat_1[11]*mat_2[8] + mat_1[15]*mat_2[12];
mat_r[13]= mat_1[3]*mat_2[1] + mat_1[7]*mat_2[5] + mat_1[11]*mat_2[9] + mat_1[15]*mat_2[13];
mat_r[14]= mat_1[3]*mat_2[2] + mat_1[7]*mat_2[6] + mat_1[11]*mat_2[10] + mat_1[15]*mat_2[14];
mat_r[15]= mat_1[3]*mat_2[3] + mat_1[7]*mat_2[7] + mat_1[11]*mat_2[11] + mat_1[15]*mat_2[15];
}// *** End TraspMatrXMatr44() ***
// ***************************************************
// --- Matrix addition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrAdd44(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0] + mat_2[0];
mat_ret[1] = mat_1[1] + mat_2[1];
mat_ret[2] = mat_1[2] + mat_2[2];
mat_ret[3] = mat_1[3] + mat_2[3];
mat_ret[4] = mat_1[4] + mat_2[4];
mat_ret[5] = mat_1[5] + mat_2[5];
mat_ret[6] = mat_1[6] + mat_2[6];
mat_ret[7] = mat_1[7] + mat_2[7];
mat_ret[8] = mat_1[8] + mat_2[8];
mat_ret[9] = mat_1[9] + mat_2[9];
mat_ret[10]= mat_1[10] + mat_2[10];
mat_ret[11]= mat_1[11] + mat_2[11];
mat_ret[12]= mat_1[12] + mat_2[12];
mat_ret[13]= mat_1[13] + mat_2[13];
mat_ret[14]= mat_1[14] + mat_2[14];
mat_ret[15]= mat_1[15] + mat_2[15];
}// *** End MatrAdd44() ***
// ***************************************************
// --- Matrix subtraction: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrSubtract44(double *mat_1, double *mat_2, double *mat_ret)
{
mat_ret[0] = mat_1[0] - mat_2[0];
mat_ret[1] = mat_1[1] - mat_2[1];
mat_ret[2] = mat_1[2] - mat_2[2];
mat_ret[3] = mat_1[3] - mat_2[3];
mat_ret[4] = mat_1[4] - mat_2[4];
mat_ret[5] = mat_1[5] - mat_2[5];
mat_ret[6] = mat_1[6] - mat_2[6];
mat_ret[7] = mat_1[7] - mat_2[7];
mat_ret[8] = mat_1[8] - mat_2[8];
mat_ret[9] = mat_1[9] - mat_2[9];
mat_ret[10]= mat_1[10] - mat_2[10];
mat_ret[11]= mat_1[11] - mat_2[11];
mat_ret[12]= mat_1[12] - mat_2[12];
mat_ret[13]= mat_1[13] - mat_2[13];
mat_ret[14]= mat_1[14] - mat_2[14];
mat_ret[15]= mat_1[15] - mat_2[15];
}// *** End MatrSubtract44() ***
// ***************************************************
// --- transposition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void trasp_mat44(double *mat_1, double *mat_r)
{
mat_r[0]=mat_1[0]; mat_r[4]=mat_1[1]; mat_r[8] =mat_1[2] ; mat_r[12]=mat_1[3] ;
mat_r[1]=mat_1[4]; mat_r[5]=mat_1[5]; mat_r[9] =mat_1[6] ; mat_r[13]=mat_1[7] ;
mat_r[2]=mat_1[8]; mat_r[6]=mat_1[9]; mat_r[10]=mat_1[10]; mat_r[14]=mat_1[11];
mat_r[3]=mat_1[12]; mat_r[7]=mat_1[13]; mat_r[11]=mat_1[14]; mat_r[15]=mat_1[15];
}// *** End trasp_mat44() ***
// ***************************************************
// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void scalemul44_ret3(double *scal, double *mat, double *mat_rr)
{// --- (var di ritorno III par) ---
mat_rr[0] = mat[0] * *scal; mat_rr[4] = mat[4] * *scal; mat_rr[8] = mat[8] * *scal; mat_rr[12]= mat[12]* *scal;
mat_rr[1] = mat[1] * *scal; mat_rr[5] = mat[5] * *scal; mat_rr[9] = mat[9] * *scal; mat_rr[13]= mat[13]* *scal;
mat_rr[2] = mat[2] * *scal; mat_rr[6] = mat[6] * *scal; mat_rr[10]= mat[10]* *scal; mat_rr[14]= mat[14]* *scal;
mat_rr[3] = mat[3] * *scal; mat_rr[7] = mat[7] * *scal; mat_rr[11]= mat[11]* *scal; mat_rr[15]= mat[15]* *scal;
}// *** End scalemul44_ret3() ***
// ***************************************************
// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr[16] (column matr) | par 1: RetVal, matr[16] (column matr)
// par 2: InpVal, var
void scalemul44_ret1(double *mat, double scale)
{// --- (var di ritorno I par) ---
mat[ 0] *= scale; mat[ 4] *= scale; mat[ 8] *= scale; mat[12] *= scale;
mat[ 1] *= scale; mat[ 5] *= scale; mat[ 9] *= scale; mat[13] *= scale;
mat[ 2] *= scale; mat[ 6] *= scale; mat[10] *= scale; mat[14] *= scale;
mat[ 3] *= scale; mat[ 7] *= scale; mat[11] *= scale; mat[15] *= scale;
}// *** End scalemul44() ***
// --- determinant: ---
// par 1: InpVal, matr[16] (column matr)
double determinant44(double *mat)
{
double value;
value =
mat[3]*mat[6]*mat[ 9]*mat[12]-mat[2]*mat[7]*mat[ 9]*mat[12]-mat[3]*mat[5]*mat[10]*mat[12]+mat[1]*mat[7]*mat[10]*mat[12]+
mat[2]*mat[5]*mat[11]*mat[12]-mat[1]*mat[6]*mat[11]*mat[12]-mat[3]*mat[6]*mat[ 8]*mat[13]+mat[2]*mat[7]*mat[ 8]*mat[13]+
mat[3]*mat[4]*mat[10]*mat[13]-mat[0]*mat[7]*mat[10]*mat[13]-mat[2]*mat[4]*mat[11]*mat[13]+mat[0]*mat[6]*mat[11]*mat[13]+
mat[3]*mat[5]*mat[ 8]*mat[14]-mat[1]*mat[7]*mat[ 8]*mat[14]-mat[3]*mat[4]*mat[ 9]*mat[14]+mat[0]*mat[7]*mat[ 9]*mat[14]+
mat[1]*mat[4]*mat[11]*mat[14]-mat[0]*mat[5]*mat[11]*mat[14]-mat[2]*mat[5]*mat[ 8]*mat[15]+mat[1]*mat[6]*mat[ 8]*mat[15]+
mat[2]*mat[4]*mat[ 9]*mat[15]-mat[0]*mat[6]*mat[ 9]*mat[15]-mat[1]*mat[4]*mat[10]*mat[15]+mat[0]*mat[5]*mat[10]*mat[15];
return(value);
}// *** End determinant44() ***
// --- matrix inversion: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void InvMatr44(double *mat, double *mat_ret)
{
double det = determinant44(mat);
mat_ret[0] = mat[ 6]*mat[11]*mat[13] - mat[ 7]*mat[10]*mat[13] + mat[ 7]*mat[ 9]*mat[14] - mat[ 5]*mat[11]*mat[14] - mat[ 6]*mat[ 9]*mat[15] + mat[ 5]*mat[10]*mat[15];
mat_ret[1] = mat[ 3]*mat[10]*mat[13] - mat[ 2]*mat[11]*mat[13] - mat[ 3]*mat[ 9]*mat[14] + mat[ 1]*mat[11]*mat[14] + mat[ 2]*mat[ 9]*mat[15] - mat[ 1]*mat[10]*mat[15];
mat_ret[2] = mat[ 2]*mat[ 7]*mat[13] - mat[ 3]*mat[ 6]*mat[13] + mat[ 3]*mat[ 5]*mat[14] - mat[ 1]*mat[ 7]*mat[14] - mat[ 2]*mat[ 5]*mat[15] + mat[ 1]*mat[ 6]*mat[15];
mat_ret[3] = mat[ 3]*mat[ 6]*mat[ 9] - mat[ 2]*mat[ 7]*mat[ 9] - mat[ 3]*mat[ 5]*mat[10] + mat[ 1]*mat[ 7]*mat[10] + mat[ 2]*mat[ 5]*mat[11] - mat[ 1]*mat[ 6]*mat[11];
mat_ret[4] = mat[ 7]*mat[10]*mat[12] - mat[ 6]*mat[11]*mat[12] - mat[ 7]*mat[ 8]*mat[14] + mat[ 4]*mat[11]*mat[14] + mat[ 6]*mat[ 8]*mat[15] - mat[ 4]*mat[10]*mat[15];
mat_ret[5] = mat[ 2]*mat[11]*mat[12] - mat[ 3]*mat[10]*mat[12] + mat[ 3]*mat[ 8]*mat[14] - mat[ 0]*mat[11]*mat[14] - mat[ 2]*mat[ 8]*mat[15] + mat[ 0]*mat[10]*mat[15];
mat_ret[6] = mat[ 3]*mat[ 6]*mat[12] - mat[ 2]*mat[ 7]*mat[12] - mat[ 3]*mat[ 4]*mat[14] + mat[ 0]*mat[ 7]*mat[14] + mat[ 2]*mat[ 4]*mat[15] - mat[ 0]*mat[ 6]*mat[15];
mat_ret[7] = mat[ 2]*mat[ 7]*mat[ 8] - mat[ 3]*mat[ 6]*mat[ 8] + mat[ 3]*mat[ 4]*mat[10] - mat[ 0]*mat[ 7]*mat[10] - mat[ 2]*mat[ 4]*mat[11] + mat[ 0]*mat[ 6]*mat[11];
mat_ret[8] = mat[ 5]*mat[11]*mat[12] - mat[ 7]*mat[ 9]*mat[12] + mat[ 7]*mat[ 8]*mat[13] - mat[ 4]*mat[11]*mat[13] - mat[ 5]*mat[ 8]*mat[15] + mat[ 4]*mat[ 9]*mat[15];
mat_ret[9] = mat[ 3]*mat[ 9]*mat[12] - mat[ 1]*mat[11]*mat[12] - mat[ 3]*mat[ 8]*mat[13] + mat[ 0]*mat[11]*mat[13] + mat[ 1]*mat[ 8]*mat[15] - mat[ 0]*mat[ 9]*mat[15];
mat_ret[10]= mat[ 1]*mat[ 7]*mat[12] - mat[ 3]*mat[ 5]*mat[12] + mat[ 3]*mat[ 4]*mat[13] - mat[ 0]*mat[ 7]*mat[13] - mat[ 1]*mat[ 4]*mat[15] + mat[ 0]*mat[ 5]*mat[15];
mat_ret[11]= mat[ 3]*mat[ 5]*mat[ 8] - mat[ 1]*mat[ 7]*mat[ 8] - mat[ 3]*mat[ 4]*mat[ 9] + mat[ 0]*mat[ 7]*mat[ 9] + mat[ 1]*mat[ 4]*mat[11] - mat[ 0]*mat[ 5]*mat[11];
mat_ret[12]= mat[ 6]*mat[ 9]*mat[12] - mat[ 5]*mat[10]*mat[12] - mat[ 6]*mat[ 8]*mat[13] + mat[ 4]*mat[10]*mat[13] + mat[ 5]*mat[ 8]*mat[14] - mat[ 4]*mat[ 9]*mat[14];
mat_ret[13]= mat[ 1]*mat[10]*mat[12] - mat[ 2]*mat[ 9]*mat[12] + mat[ 2]*mat[ 8]*mat[13] - mat[ 0]*mat[10]*mat[13] - mat[ 1]*mat[ 8]*mat[14] + mat[ 0]*mat[ 9]*mat[14];
mat_ret[14]= mat[ 2]*mat[ 5]*mat[12] - mat[ 1]*mat[ 6]*mat[12] - mat[ 2]*mat[ 4]*mat[13] + mat[ 0]*mat[ 6]*mat[13] + mat[ 1]*mat[ 4]*mat[14] - mat[ 0]*mat[ 5]*mat[14];
mat_ret[15]= mat[ 1]*mat[ 6]*mat[ 8] - mat[ 2]*mat[ 5]*mat[ 8] + mat[ 2]*mat[ 4]*mat[ 9] - mat[ 0]*mat[ 6]*mat[ 9] - mat[ 1]*mat[ 4]*mat[10] + mat[ 0]*mat[ 5]*mat[10];
scalemul44_ret1(mat_ret, 1/det);
}// *** End InvMatr44() ***
// ******************************************************
// --- normalize a vector (ok also for quaternions): ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: RetVal, vect [0],[1],[2],[3]
void NormalizeVers4(double *quat, double *vec_r)
{
double length, ilength;
length = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3] ;
length = sqrt(length);
if ( length > 0.0)
{
ilength = 1.0/length;
vec_r[0] = quat[0] * ilength;
vec_r[1] = quat[1] * ilength;
vec_r[2] = quat[2] * ilength;
vec_r[3] = quat[3] * ilength;
}
}// *** End NormalizeVers4() ***
// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************
// --- direct transformation of a vector: (Vec3XtraspMatr33) ---
// par 1: ImpVal, vec [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------- cosines ----
// matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
/* #define transf_dir_vec33 Vec3XtraspMatr33 --- in matrixe.h --- */
// --- inverse transformation of a vector: (Vec3XMatr33) ---
// par 1: ImpVal, vec [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------- cosines ----
// matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
/* #define transf_inv_vec33 Vec3XMatr33 --- in matrixe.h --- */
// --- direct transformation of a point: (sub, Vec3XtraspMatr33) ---
// par 1: ImpVal, point [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_dir_pt331(double *pt_input, double *matrix, double *pt_return)
{
double pt_temp[4];
pt_temp[0]=pt_input[0] - matrix[12];
pt_temp[1]=pt_input[1] - matrix[13];
pt_temp[2]=pt_input[2] - matrix[14];
Vec3XtraspMatr33(pt_temp, matrix, pt_return);
}// *** End transf_dir_pt3() ***
// --- inverse transformation of a point: (Vec3XMatr33, add) ---
// par 1: ImpVal, point [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_inv_pt331(double *pt_input, double *matrix, double *pt_return)
{
double pt_temp[4];
Vec3XMatr33(pt_input, matrix, pt_temp);
pt_return[0]=pt_temp[0] + matrix[12];
pt_return[1]=pt_temp[1] + matrix[13];
pt_return[2]=pt_temp[2] + matrix[14];
}// *** End transf_inv_pt3() ***
// --- direct transformation of a matrix: MatrXtraspMatr33,
// (sub, Vec3XtraspMatr33) ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: InpVal, matr[16]
// par 3: RetVal, matr[16]
void transf_dir_axs331(double *matrix1, double *matrix2, double *matrix_ret)
{
double pt_temp[4];
MatrXtraspMatr33(matrix1, matrix2, matrix_ret);
transf_dir_pt331(&matrix1[12], matrix2, pt_temp);
matrix_ret[12]=pt_temp[0]; matrix_ret[13]=pt_temp[1]; matrix_ret[14]=pt_temp[2];
}// *** End transf_dir_axs33() ***
// --- inverse transformation of a matrix: MatrXMatr33,
// (Vec3XMatr33, add) ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: InpVal, matr[16]
// par 3: RetVal, matr[16]
void transf_inv_axs331(double *matrix1, double *matrix2, double *matrix_ret)
{
double pt_temp[4];
MatrXMatr33(matrix1, matrix2, matrix_ret);
transf_inv_pt331(&matrix1[12], matrix2, pt_temp);
matrix_ret[12]=pt_temp[0]; matrix_ret[13]=pt_temp[1]; matrix_ret[14]=pt_temp[2];
}// *** End transf_inv_axs33() ***
// --- Invert definition of an axis system: ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: RetVal, matr[16]
void inv_axs_def331(double *matrix, double *matrix_ret)
{
transf_dir_axs331((double *)matrix_id, matrix, matrix_ret);
}// *** End transf_dir_axs33() ***
// --- return identity matrix: ---
// par 1: InpVal, matrix declared matr[16]
// matr [0],[4],[8] ,[12]
// [1],[5],[9] ,[13]
// [2],[6],[10],[14]
// [3],[7],[11],[15]
//
// par 1: RetVal, 1 0 0 0
// 0 1 0 0
// 0 0 1 0
// 0 0 0 1
void MatrIdentify( double *matrix )
{
matrix[0] = 1.0; matrix[4] = 0.0; matrix[8] = 0.0; matrix[12] = 0.0;
matrix[1] = 0.0; matrix[5] = 1.0; matrix[9] = 0.0; matrix[13] = 0.0;
matrix[2] = 0.0; matrix[6] = 0.0; matrix[10] = 1.0; matrix[14] = 0.0;
matrix[3] = 0.0; matrix[7] = 0.0; matrix[11] = 0.0; matrix[15] = 1.0;
}// *** End MatrIdentify() ***
// --- print on console (printf()) a matrix: ---
// --- mode(par1): 1) 3x3,col 2) 3x3,row 3) 4x4,col 4) 4x4,row
// matrix declared matr[16]
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// If par 1 = 3 o par 1 = 4 par 2 will be all the matrix[16]
void print_matr(int mode, double *mat)
{// mode: 1) 3x3,col 2) 3x3,row 3) 4x4,col 4) 4x4,row
switch(mode)
{
case 1: // 3x3,col
{
printf("(column vers)
");
printf(" %.10f, %.10f, %.10f
", mat[0], mat[4], mat[8]);
printf(" %.10f, %.10f, %.10f
", mat[1], mat[5], mat[9]);
printf(" %.10f, %.10f, %.10f
", mat[2], mat[6], mat[10]);
break;
}
case 2: // 3x3,row
{
printf("(row vers)
");
printf(" %.10f, %.10f, %.10f
", mat[0], mat[1], mat[2]);
printf(" %.10f, %.10f, %.10f
", mat[4], mat[5], mat[6]);
printf(" %.10f, %.10f, %.10f
", mat[8], mat[9], mat[10]);
break;
}
case 3: // 4x4,col
{
printf("(column vers)
");
printf(" %.10f, %.10f, %.10f, %.10f
", mat[0], mat[4], mat[8] , mat[12]);
printf(" %.10f, %.10f, %.10f, %.10f
", mat[1], mat[5], mat[9] , mat[13]);
printf(" %.10f, %.10f, %.10f, %.10f
", mat[2], mat[6], mat[10], mat[14]);
printf(" %.10f, %.10f, %.10f, %.10f
", mat[3], mat[7], mat[11], mat[15]);
break;
}
case 4: // 4x4,row
{
printf("(row vers)
");
printf(" %.10f, %.10f, %.10f, %.10f
", mat[0] , mat[1] , mat[2] , mat[3] );
printf(" %.10f, %.10f, %.10f, %.10f
", mat[4] , mat[5] , mat[6] , mat[7] );
printf(" %.10f, %.10f, %.10f, %.10f
", mat[8] , mat[9] , mat[10], mat[11]);
printf(" %.10f, %.10f, %.10f, %.10f
", mat[12], mat[13], mat[14], mat[15]);
break;
}
}// *** End switch(mode) ***
}// *** End print_matr() ***
// ***************************************************
// --- print on console (printf()) a vector (3 o 4 elements)
// --- mode(par1): 1) vect3,col 2) vect3,row 3) vect4,col 4) vect4,row
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, vect [0],[1],[2]
// If par 1 = 3 o par 1 = 4 par 2 will be all vect[4]
void print_vers(int mode, double *vers)
{// mode: 1) 3,col 2) 3,row 3) 4,col 4) 4,row
switch(mode)
{
case 1: // 3,col
{
printf(" %.10f
%.10f
%.10f
", vers[0], vers[1], vers[2]);
break;
}
case 2: // 3,row
{
printf(" %.10f, %.10f, %.10f
", vers[0], vers[1], vers[2]);
break;
}
case 3: // 4,col
{
printf(" %.10f
%.10f
%.10f
%.10f
", vers[0], vers[1], vers[2], vers[3]);
break;
}
case 4: // 4,row
{
printf(" %.10f, %.10f, %.10f, %.10f
", vers[0], vers[1], vers[2], vers[3]);
break;
}
}// *** End switch(mode) ***
}// *** End print_vers() ***
// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */
// --- distance between 2 points xy(2D): ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, &var
void dist_pt2pt2(double *pt_1, double *pt_2, double *distptpt)
{
*distptpt = sqrt((pt_2[0]-pt_1[0])*(pt_2[0]-pt_1[0]) + (pt_2[1]-pt_1[1])*(pt_2[1]-pt_1[1]));
}// *** End dist_pt2pt2() ***
// ***************************************************
// --- distance between 2 points xyz(3D): ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dist_pt3pt3(double *pt_1, double *pt_2, double *distptpt)
{
*distptpt = sqrt((pt_2[0]-pt_1[0])*(pt_2[0]-pt_1[0]) +
(pt_2[1]-pt_1[1])*(pt_2[1]-pt_1[1]) +
(pt_2[2]-pt_1[2])*(pt_2[2]-pt_1[2]));
}// *** End dist_pt3pt3() ***
// ***************************************************
// --- intersection of 2 circ(2D): 1° circ: x,y center and radius.
// --- 2° circ: x,y center and radius.
// --- Return in x1,y1 e x2,y2 ---
// par 1: InpVal, &var | par 1: RetVal, &var
// par 2: InpVal, &var | par 2: RetVal, &var
// par 3: InpVal, var
// par 4: InpVal, &var | par 4: RetVal, &var
// par 5: InpVal, &var | par 5: RetVal, &var
// par 6: InpVal, var
int c2c (double *x1, double *y1, double r1, double *x2, double *y2, double r2)
{
double xx1=0, yy1=0, xx2=0, yy2=0;
double rmax=0,rmin=0;
double cdist=0;
double vara=0,varh=0,varpX=0,varpY=0,varb=0;
xx1=*x1;yy1=*y1;xx2=*x2;yy2=*y2;
rmax=r1+r2;
rmin=fabs(r1-r2);
cdist=sqrt((xx2-xx1)*(xx2-xx1) + (yy2-yy1)*(yy2-yy1));
if(cdist>rmax)
return 1;
else if(cdist<rmin)
return 2;
else
{
vara=(r1 * r1 - r2 * r2 + cdist*cdist) / (2 * cdist);
varh=sqrt(r1 * r1 - vara * vara);
//lerp:
varpX=(xx1 + (xx2 - xx1) * (vara/cdist));
varpY=(yy1 + (yy2 - yy1) * (vara/cdist));
varb=varh/cdist;
*x1=varpX - varb * (yy2 - yy1);
*y1=varpY + varb * (xx2 - xx1);
*x2=varpX + varb * (yy2 - yy1);
*y2=varpY - varb * (xx2 - xx1);
return 0;
}
}// *** End c2c() ***
// ***************************************************
// --- triangle angles, if we know sides. ---
// par 1: InpVal, var
// par 2: InpVal, var
// par 3: InpVal, var
// par 4: RetVal, vect [0],[1],[2]
void tri(double lato_a, double lato_b, double lato_c, double *angles )
{/*
/|
/ |
/ta|
/ |
/ |
c/ |b
/ |
/ |
/tb tc|
----------
a */
angles[0]=TODEGR * acos((lato_b*lato_b+lato_c*lato_c-lato_a*lato_a)/(2*lato_b*lato_c)); // ta
angles[1]=TODEGR * acos((lato_a*lato_a+lato_c*lato_c-lato_b*lato_b)/(2*lato_a*lato_c)); // tb
angles[2]=TODEGR * acos((lato_a*lato_a+lato_b*lato_b-lato_c*lato_c)/(2*lato_a*lato_b)); // tc
}// *** End tri() ***
// ***************************************************
// --- 4 points on a sphere, returns center (xyz) and radius: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: InpVal, vect [0],[1],[2]
// par 5: RetVal, vect [0],[1],[2]
// par 6: RetVal, &var
int sfera(double *pt_1, double *pt_2, double *pt_3, double *pt_4, double *ptc, double *radius)
{
double sfmat[16];
double m11,m12,m13,m14,m15;
/* Find determinant M11 */
sfmat[0] = pt_1[0]; sfmat[4] = pt_1[1]; sfmat[ 8] = pt_1[2]; sfmat[12] = 1;
sfmat[1] = pt_2[0]; sfmat[5] = pt_2[1]; sfmat[ 9] = pt_2[2]; sfmat[13] = 1;
sfmat[2] = pt_3[0]; sfmat[6] = pt_3[1]; sfmat[10] = pt_3[2]; sfmat[14] = 1;
sfmat[3] = pt_4[0]; sfmat[7] = pt_4[1]; sfmat[11] = pt_4[2]; sfmat[15] = 1;
m11 = determinant44(sfmat);
/* Find determinant M12 */
sfmat[0] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
sfmat[4] = pt_1[1];
sfmat[8] = pt_1[2];
sfmat[12]= 1;
sfmat[1] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
sfmat[5] = pt_2[1];
sfmat[9] = pt_2[2];
sfmat[13]= 1;
sfmat[2] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
sfmat[6] = pt_3[1];
sfmat[10]= pt_3[2];
sfmat[14]= 1;
sfmat[3] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
sfmat[7] = pt_4[1];
sfmat[11]= pt_4[2];
sfmat[15]= 1;
m12 = determinant44(sfmat);
/* Find determinant M13 */
sfmat[0] = pt_1[0];
sfmat[4] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
sfmat[8] = pt_1[2];
sfmat[12]= 1;
sfmat[1] = pt_2[0];
sfmat[5] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
sfmat[9] = pt_2[2];
sfmat[13]= 1;
sfmat[2] = pt_3[0];
sfmat[6] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
sfmat[10]= pt_3[2];
sfmat[14]= 1;
sfmat[3] = pt_4[0];
sfmat[7] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
sfmat[11]= pt_4[2];
sfmat[15]= 1;
m13 = determinant44(sfmat);
/* Find determinant M14 */
sfmat[0] = pt_1[0];
sfmat[4] = pt_1[1];
sfmat[8] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
sfmat[12]= 1;
sfmat[1] = pt_2[0];
sfmat[5] = pt_2[1];
sfmat[9] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
sfmat[13]= 1;
sfmat[2] = pt_3[0];
sfmat[6] = pt_3[1];
sfmat[10]= pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
sfmat[14]= 1;
sfmat[3] = pt_4[0];
sfmat[7] = pt_4[1];
sfmat[11]= pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
sfmat[15]= 1;
m14 = determinant44(sfmat);
/* Find determinant M15 */
sfmat[0] = pt_1[0]*pt_1[0] + pt_1[1]*pt_1[1] + pt_1[2]*pt_1[2];
sfmat[4] = pt_1[0];
sfmat[8] = pt_1[1];
sfmat[12]= pt_1[2];
sfmat[1] = pt_2[0]*pt_2[0] + pt_2[1]*pt_2[1] + pt_2[2]*pt_2[2];
sfmat[5] = pt_2[0];
sfmat[9] = pt_2[1];
sfmat[13]= pt_2[2];
sfmat[2] = pt_3[0]*pt_3[0] + pt_3[1]*pt_3[1] + pt_3[2]*pt_3[2];
sfmat[6] = pt_3[0];
sfmat[10]= pt_3[1];
sfmat[14]= pt_3[2];
sfmat[3] = pt_4[0]*pt_4[0] + pt_4[1]*pt_4[1] + pt_4[2]*pt_4[2];
sfmat[7] = pt_4[0];
sfmat[11]= pt_4[1];
sfmat[15]= pt_4[2];
m15 = determinant44(sfmat);
fprintf(stderr,"Determinants: %g %g %g %g %g
",m11,m12,m13,m14,m15);
if (m11 == 0) {
fprintf(stderr,"The points don't define a sphere!
");
ptc[0] = 0.0;
ptc[1] = 0.0;
ptc[2] = 0.0;
*radius = 0.0;
return(1);
}
ptc[0] = 0.5 * m12 / m11;
ptc[1] = 0.5 * m13 / m11;
ptc[2] = 0.5 * m14 / m11;
*radius = sqrt(ptc[0]*ptc[0] + ptc[1]*ptc[1] + ptc[2]*ptc[2] - m15/m11);
fprintf(stderr,"Sphere Center: %g,%g,%g Radius: %g
", ptc[0], ptc[1], ptc[2], *radius);
return(0);
}// *** End sfera() ***
// ***************************************************
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- distance point - line (3d). ---
// double line[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, line [0],[1],[2] --- start pt
// [4],[5],[6] --- end pt
// par 3: RetVal, var --- distance
// par 4: RetVal, vect [0],[1],[2] --- pt on the line
void dist_Point_to_Line( double *P, double *L, double *dist, double *line_pt)
{
double vecV[4], vecW[4];
double cos1, cos2, b;
vecV[0] = L[4]-L[0];
vecV[1] = L[5]-L[1];
vecV[2] = L[6]-L[2];
vecW[0] = P[0]-L[0];
vecW[1] = P[1]-L[1];
vecW[2] = P[2]-L[2];
dotprod(vecW, vecV, &cos1);
dotprod(vecV, vecV, &cos2);
b = cos1 / cos2;
line_pt[0] = L[0]+ b * vecV[0];
line_pt[1] = L[1]+ b * vecV[1];
line_pt[2] = L[2]+ b * vecV[2];
dist_pt3pt3(P, line_pt, dist);
}// *** End dist_Point_to_Line() ***
// ***************************************************
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// get base of perpendicular from point to a plane
// Input: P = a 3D point
// PL = a plane with point V0 and normal n
// Output: *B(pt_on_pln) = base point on PL of perpendicular from P
// Return: the distance from P to the plane PL
//
// double plane[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, plane [0],[1],[2] x,y,z pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2]
// par 4: RetVal, var distance
void dist_Point_to_Plane( double *P, double *PL, double *pt_on_pln, double *dst)
{
double cos1, cos2, sb;
double pt_t[4];
pt_t[0] = P[0] - PL[0];
pt_t[1] = P[1] - PL[1];
pt_t[2] = P[2] - PL[2];
dotprod(&PL[4], pt_t, &cos1);
dotprod(&PL[4], &PL[4], &cos2);
sb = (-cos1) / cos2;
pt_on_pln[0] = P[0]+ sb * PL[4];
pt_on_pln[1] = P[1]+ sb * PL[5];
pt_on_pln[2] = P[2]+ sb * PL[6];
dist_pt3pt3(P, pt_on_pln, dst);
}// *** End dist_Point_to_Plane() ***
// ***************************************************
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- Input: two 3D lines L1 and L2 --- Return: the shortest distance ---
// double line[8]; // [0],[1],[2] x,y,z start pt [4],[5],[6] end pt
// par 1: InpVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 2: InpVal, line [0],[1],[2]
// [4],[5],[6] end pt
// par 3: RetVal, &var
// par 4: RetVal, &var
// par 5: RetVal, point [0],[1],[2]
// par 6: RetVal, point [0],[1],[2]
// Return: the shortest distance between L1 and L2,
// lines parameters in vL1par and vL2par
// and nearest points in pt_1 and pt_2
double dist3D_Line_to_Line( double *vL1, double *vL2, double *vL1par, double *vL2par, double *pt_1, double *pt_2)
{
double vec_u[4], vec_v[4], vec_w[4], dP[4], vec_tmp1[4], vec_tmp2[4], vec_tmp3[4];
double a,b,c,d,e,D,retv;
double vL1len, vL1vers[4];
double vL2len, vL2vers[4];
V3D_Sub(&vL1[4], vL1, vec_u);
V3D_Sub(&vL2[4], vL2, vec_v);
V3D_Sub(vL1, vL2, vec_w);
dotprod(vec_u, vec_u, &a); // always >= 0
dotprod(vec_u, vec_v, &b);
dotprod(vec_v, vec_v, &c); // always >= 0
dotprod(vec_u, vec_w, &d);
dotprod(vec_v, vec_w, &e);
D = a*c - b*b; // always >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM) { // the lines are almost parallel
*vL1par = 0.0;
*vL2par = (b>c ? d/b : e/c); // use the largest denominator
}
else {
*vL1par = (b*e - c*d) / D;
*vL2par = (a*e - b*d) / D;
}
dist_pt3pt3(vL1, &vL1[4], &vL1len);
dist_pt3pt3(vL2, &vL2[4], &vL2len);
vL1len = vL1len * (*vL1par); // distance start pt - nearest pt
vL2len = vL2len * (*vL2par);
s2vrs(vL1, &vL1[4], vL1vers);
s2vrs(vL2, &vL2[4], vL2vers);
pt_1[0] = vL1[0] + vL1vers[0]*vL1len; pt_1[1] = vL1[1] + vL1vers[1]*vL1len; pt_1[2] = vL1[2] + vL1vers[2]*vL1len;
pt_2[0] = vL2[0] + vL2vers[0]*vL2len; pt_2[1] = vL2[1] + vL2vers[1]*vL2len; pt_2[2] = vL2[2] + vL2vers[2]*vL2len;
// get the difference of the two closest points
/// Vector dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
vec_tmp1[0] = *vL1par * vec_u[0]; vec_tmp1[1] = *vL1par * vec_u[1]; vec_tmp1[2] = *vL1par * vec_u[2];
V3D_Add(vec_w, vec_tmp1, vec_tmp2);
vec_tmp3[0] = *vL2par * vec_v[0]; vec_tmp3[1] = *vL2par * vec_v[1]; vec_tmp3[2] = *vL2par * vec_v[2];
V3D_Sub(vec_tmp2, vec_tmp3, dP);
V3D_Length(dP, &retv);
return retv;
}// *** End dist3D_Line_to_Line() ***
// ***************************************************
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_SegmentPlane(): intersect a segment and a plane
// Input: S = a segment, and Pn = a plane = {Point V0; Vector n;}
// Output: *I0 = the intersect point (when it exists)
// Return: 0 = disjoint (no intersection)
// 1 = intersection in the unique point *I0
// 2 = the segment lies in the plane
//
// double segm[8]; double plan[8]; double pnt[4]; int *retval;
// par 1: InpVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2] inters. point
// par 4: RetVal, &var return flag
void inters_ln_pln( double *segm, double *plan, double *pnt, int *retval)
{
double u[4];
double v[4];
double D, N, interspar;
u[0] = segm[4] - segm[0]; u[1] = segm[5] - segm[1]; u[2] = segm[6] - segm[2];
v[0] = segm[0] - plan[0]; v[1] = segm[1] - plan[1]; v[2] = segm[2] - plan[2];
dotprod(&plan[4], u, &D);
dotprod(&plan[4], v, &N);
if (fabs(D) < 0.000000001) // segment is parallel to plane
{
if (N == 0) // segment lies in plane
*retval = 2;
else
*retval = 0; // no intersection
}
// they are not parallel
// compute intersect param
interspar = (-N) / D;
if (interspar < 0 || interspar > 1)
*retval = 0; // no intersection
// compute segment intersect point
pnt[0] = segm[0] + interspar * u[0];
pnt[1] = segm[1] + interspar * u[1];
pnt[2] = segm[2] + interspar * u[2];
*retval = 1;
}// *** End inters_ln_pln() ***
// ***************************************************
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_2Planes(): the 3D intersect of two planes
// Input: two planes Pn1 and Pn2
// Output: *L = the intersection line (when it exists)
// Return: 0 = disjoint (no intersection)
// 1 = the two planes coincide
// 2 = intersection in the unique line *L
// par 1: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 4: RetVal, &var return flag
void inters_pln_pln(double *pln1, double *pln2, double *ln, int *retval)
{
double u[4], v[4];
double ax, ay, az, cos1;
double d1, d2; // the constants in the 2 plane equations
int maxc; // max coordinate
crossprod(&pln1[4], &pln2[4], u);
ax = (u[0] >= 0 ? u[0] : -u[0]);
ay = (u[1] >= 0 ? u[1] : -u[1]);
az = (u[2] >= 0 ? u[2] : -u[2]);
// test if the two planes are parallel
if ((ax+ay+az) < 0.000000001) // Pn1 and Pn2 are near parallel
{
// test if disjoint or coincide
v[0] = pln2[0] - pln1[0];
v[1] = pln2[1] - pln1[1];
v[2] = pln2[2] - pln1[2];
dotprod(&pln1[4], v, &cos1);
if(cos1 == 0) // pln2 pt lies in pln1
*retval = 1; // pln1 and pln2 coincide
else
*retval = 0; // pln1 and pln2 are disjoint
}
// Pn1 and Pn2 intersect in a line
// first determine max abs coordinate of cross product
if (ax > ay)
{
if (ax > az) maxc = 1;
else maxc = 3;
}
else
{
if (ay > az) maxc = 2;
else maxc = 3;
}
// next, to get a point on the intersect line
// zero the max coord, and solve for the other two
dotprod(&pln1[4], pln1, &d1); d1 = -d1; // note: could be pre-stored with plane
dotprod(&pln2[4], pln2, &d2); d2 = -d2; // ditto
switch (maxc) // select max coordinate
{
case 1: // intersect with x=0 (ln[0], [1], [2] intersection point)
ln[0] = 0;
ln[1] = (d2*pln1[6] - d1*pln2[6]) / u[0];
ln[2] = (d1*pln2[5] - d2*pln1[5]) / u[0];
break;
case 2: // intersect with y=0
ln[0] = (d1*pln2[6] - d2*pln1[6]) / u[1];
ln[1] = 0;
ln[2] = (d2*pln1[4] - d1*pln2[4]) / u[1];
break;
case 3: // intersect with z=0
ln[0] = (d2*pln1[5] - d1*pln2[5]) / u[2];
ln[1] = (d1*pln2[4] - d2*pln1[4]) / u[2];
ln[2] = 0;
}
ln[4] = ln[0] + u[0]; ln[5] = ln[1] + u[1]; ln[6] = ln[2] + u[2];
*retval = 2;
}// *** End inters_pln_pln() ***
// --- Geometric center of gravity of a triangle, knowing the vertices: ---
// par 1: InpVal, pt [0],[1],[2]
// par 2: InpVal, pt [0],[1],[2]
// par 3: RetVal, pt [0],[1],[2]
// par 4: RetVal, pt [0],[1],[2]
void tri_centroid(double *pt_1, double *pt_2, double *pt_3, double *pt_return)
{
pt_return[0] = (pt_1[0] + pt_2[0] + pt_3[0]) / 3.0;
pt_return[1] = (pt_1[1] + pt_2[1] + pt_3[1]) / 3.0;
pt_return[2] = (pt_1[2] + pt_2[2] + pt_3[2]) / 3.0;
}// *** End tri_centroid() ***
// ***************************************************
// ***************************************************
// ***************************************************
/***** Serialization - deSerialization: **************
--------------------------------------------
| mat[0] mat[4] mat[ 8] mat[12] |
M = | mat[1] mat[5] mat[ 9] mat[13] |
| mat[2] mat[6] mat[10] mat[14] |
| mat[3] mat[7] mat[11] mat[15] |
--------------------------------------------
----------------------------------------------
| mat[0][0] mat[0][1] mat[0][2] mat[0][3] |
M = | mat[1][0] mat[1][1] mat[1][2] mat[1][3] |
| mat[2][0] mat[2][1] mat[2][2] mat[2][3] |
| mat[3][0] mat[3][1] mat[3][2] mat[3][3] |
----------------------------------------------
The doubly dimensioned array A is treated as a one dimensional
vector, stored by COLUMNS. Entry A(I,J) is stored as A[I+J*N]
*****************************************************/
and matrixe.h:
/***********************************************
***** matrixe.h - Giovanni Rosso *****
***** rogx@libero.it *****
***** for matrixe.c v 0.67 20121118 *****
************************************************
(SORRY FOR MY ENGLISH!)
OpenGl standards: Column direction cosines:
axs X axs Y axs Z OriginiXYZ
--------------------------------------
| cosX cosX cosX X |
M = | cosY cosY cosY Y |
| cosZ cosZ cosZ Z |
| |
| 0 0 0 1 |
--------------------------------------
Serialized matrix (by columns)
The matrix uses the following positions:
--------------------------------------
| mat[0] mat[4] mat[ 8] mat[12] |
M = | mat[1] mat[5] mat[ 9] mat[13] |
| mat[2] mat[6] mat[10] mat[14] |
| |
| mat[3] mat[7] mat[11] mat[15] |
--------------------------------------
*************************************************************
Projection transformations(gluPerspective, glOrtho,
glMatrixMode(GL_PROJECTION)) are left handed.
Everything else is right handed, including vertexes
(glMatrixMode(GL_MODELVIEW)).
************************************************************
Rem: to call the subroutines that work with matrices declare
all matrices this way: double my_matrix[16];
If in the name of the sub You find "33" the operations will
use only the orientation part, ie:
mat[0] mat[4] mat[ 8]
mat[1] mat[5] mat[ 9]
mat[2] mat[6] mat[10]
// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declared as: "double my_vec[n];" (n min 2)
// ***********************************************************************
// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 3)
// ******* matrix declared as: double my_matrix[16];
// ******* that indices will be used: [0] [4] [ 8]
// ******* [1] [5] [ 9]
// ******* [2] [6] [10]
// *****************************************************************
// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4)
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************
// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************
// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */
#include <stdio.h>
#include <math.h>
#define TODEGR 57.295779513082322864647721871733665466308593750
#define TORAD 0.01745329251994329547437168059786927187815308570861816406250
// 3.14159265358979323846264338327950288419716939937510582097494459230781640628620899862803482534211706
#define PGRECO 3.14159265358979323846264338327950288419716939937510582097494
#define PHIaur 1.61803398874989490252573887119069695472717285156250
// --- anything that avoids division overflow: ---
#define SMALL_NUM 0.0000000001
// --- round a to nearest int: ---
#define ROUND(a) ((a)>0 ? (int)(a+0.5) : -(int)(0.5-a))
// ******* SECTION 1): ***************************************************
// ******* 2D vectors (only [0] e [1])
// ******* declare as: "double my_vec[n];" (n min 2)
// ***********************************************************************
// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_SquaredLength(double *, double *);
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, &var
void V2D_Length(double *, double *);
// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Add(double *, double *, double *);
// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, vect [0],[1]
void V2D_Sub(double *, double *, double *);
// --- return the vector perpendicular to the input vector vers_1: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void V2D_MakePerpendicular(double *, double *);
// --- normalizes the input vector: ---
// par 1: InpVal, vect [0],[1]
// par 2: RetVal, vect [0],[1]
void NormalizeVers2D(double *, double *);
// ******* SECTION 2): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2])
// ******* or matrix[16]
// ******* declare vec as: double my_vec[n]; (n min 3)
// ******* declare matrix as: double my_matrix[16];
// ******* [0] [4] [ 8]
// ******* [1] [5] [ 9]
// ******* [2] [6] [10] will be used.
// *****************************************************************
// --- Vector (or XYZ point) * matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XMatr33(double *, double *, double *);
// --- Vector (or xyz point) * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, vect [0],[1],[2]
void Vec3XtraspMatr33(double *, double *, double *);
// --- matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrXMatr33(double *, double *, double *);
// --- matrix * transposed matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrXtraspMatr33(double *, double *, double *);
// --- transposed matrix * matrix: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void TraspMatrXMatr33(double *, double *, double *);
// --- Matrix addition: ---
// Matrix addition is commutative.
// Matrix addition is associative.
// This means that ( a + b ) + c = a + ( b + c ).
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrAdd33(double *, double *, double *);
// --- Matrix subtraction: ---
// Matrix subtraction is NOT commutative.
// This means that you can't change the order when doing a - b.
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrSubtract33(double *, double *, double *);
// --- transposition: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void trasp_mat33(double *, double *);
// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void scalemul33_ret3(double *, double *, double *);
// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr [0],[4],[8] | par 1: RetVal, matr [0],[4],[8]
// [1],[5],[9] | [1],[5],[9]
// [2],[6],[10] | [2],[6],[10]
// par 2: InpVal, var
void scalemul33_ret1(double *, double);
// --- determinant: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
double determinant33(double *);
// --- matrix inversion: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void InvMatr33(double *, double *);
// --- normalize a vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, vect [0],[1],[2]
void NormalizeVers3D(double *, double *);
// ***************************************************
// *** 3 routines for matrix orthonormalization:
// *** double matr[16], orientation part:
// *** [0],[4],[8]
// *** [1],[5],[9]
// *** [2],[6],[10]
// *** attention to speed!
// *** benchmarks (1000000 calls):
// *** matr_reortho33 elapsed: 1.618701 --- "best fit" orthonormalization ---
// *** matr_orthonorm33 elapsed: 0.189059 --- "hierarchical" orthonormalization ---
// *** reorthonormalize33 elapsed: 0.076302 --- "casual" orthonormalization ---
// --- "best fit" orthonormalization: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void matr_reortho33(double *, double *);
// --- "hierarchical" orthonormalization: ---
// --- par. 1, int, is a flag, meaning:
// --- 1=XYZ 2=XZY 3=YZX 4=YXZ 5=ZXY 6=ZYX
// --- (I axs unchanged, II forced to 90 degrees on the plane, III calculated)
// par 1: InpVal, var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void matr_orthonorm33(int, double *, double *);
// --- "casual" orthonormalization (best speed): ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 1: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void reorthonormalize33(double *);
// --- returns squared length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_SquaredLength(double *, double *);
// --- returns length of input vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: RetVal, &var
void V3D_Length(double *, double *);
// --- return vector sum vers_r = vers_1 + vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Add(double *, double *, double *);
// --- return vector difference vers_r = vers_1 - vers_2: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void V3D_Sub(double *, double *, double *);
// --- make a linear combination of two vectors and return the result. ---
// --- result = (avect * ascl) + (bvect * bscl) ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, &var
// par 4: InpVal, &var
// par 5: RetVal, vect [0],[1],[2]
void V3D_Combine (double *, double *, double *, double *, double *);
// --- Find vector of the segment from point A (xyz) to point B (xyz): ---
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, point [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void s2vrs(double *, double *, double *);
// --- angle between 2 vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotang (double *, double *, double *);
// --- like dotang(), but returns angle in radians: ---
void dotang_rad(double *, double *, double *);
// --- cosine of a vector on another vector: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dotprod (double *, double *, double *);
// --- return a vector normal to the plane of the 2 input vectors: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void crossprod(double *, double *, double *);
// --- returns the rotation matrix for an angle around X/Y/Z: ---
// par 1: InpVal, &var
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void MatrRotX(double *, double *);
void MatrRotY(double *, double *);
void MatrRotZ(double *, double *);
// --- rotation of a vector (or a xyz point) around X/Y/Z axs: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, vect [0],[1],[2]
void RotVecX(double *, double *, double *);
void RotVecY(double *, double *, double *);
void RotVecZ(double *, double *, double *);
// --- rotation of a matrix around X/Y/Z axs (only orientation part): ---
// par 1: InpVal, &var
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 3: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void RotMatrX(double *, double *, double *);
void RotMatrY(double *, double *, double *);
void RotMatrZ(double *, double *, double *);
/* If You want to call Matr33Slerp() o QuatSlerp()
You have to allocate a "MatrixSlerp" struct: */
typedef struct
{
double mat_from[16];
double mat_to[16];
double quat_from[4];
double quat_to[4];
double percent;
double quat_r[4];
double mat_r[16];
}MatrixSlerp;
// --- linear interpolation between vectors: ---
// par 1: InpVal, &var
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: RetVal, vect [0],[1],[2]
void Vec3Lerp(double *, double *, double *, double *);
// --- spherical interpolation between matrices: ---
// par 1: allocate a "MatrixSlerp" struct
void Matr33Slerp(MatrixSlerp *);
// --- spherical interpolation between quaternions: ---
// par 1: allocate a "MatrixSlerp" struct
void QuatSlerp(MatrixSlerp *);
// --- from direct cosines matrix to Euler angles: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2]
void Dcm33ToEul(double *, double *);
// --- from Euler angles to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2]
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void EulToDcm33(double *, double *);
// --- from direct cosines matrix to quaternions: ---
// par 1: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// par 2: RetVal, vect [0],[1],[2],[3] declare: "double miovers[n];" (n min 4)
void Matr33ToQuat(double *, double *);
// --- from quaternions to direct cosines matrix: ---
// par 1: ImpVal, vect [0],[1],[2],[3] declare.: "double my_vect[n];" (n min 4)
// par 2: RetVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
void QuatToMatr33(double *, double *);
// ******* SECTION 3): *********************************************
// ******* 3D vectors or xyz points ([0] , [1], [2], [3])
// ******* or matrix[16]
// ******* vecs declared as: double my_vec[n]; (n min 4)
// ******* matrix declared as: double my_matrix[16];
// ******* all indices will be used.
// *****************************************************************
// --- vector * matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XMatr44(double *, double *, double *);
// --- vector * transposed matrix: ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: InpVal, matr [n 16] (column matr)
// par 3: RetVal, vect [0],[1],[2],[3]
void Vec4XtraspMatr44(double *, double *, double *);
// --- matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXMatr44(double *, double *, double *);
// --- matrix * transposed matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrXtraspMatr44(double *, double *, double *);
// --- transposed matrix * matrix: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void TraspMatrXMatr44(double *, double *, double *);
// --- Matrix addition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrAdd44(double *, double *, double *);
// --- Matrix subtraction: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void MatrSubtract44(double *, double *, double *);
// --- transposition: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void trasp_mat44(double *, double *);
// --- matrix * scalar: ---
// --- (ret on III par): ---
// par 1: InpVal, &var
// par 2: InpVal, matr[16] (column matr)
// par 3: RetVal, matr[16] (column matr)
void scalemul44_ret3(double *, double *, double *);
// --- matrix * scalar: ---
// --- (ret on I par): ---
// par 1: InpVal, matr[16] (column matr) | par 1: RetVal, matr[16] (column matr)
// par 2: InpVal, var
void scalemul44_ret1(double *, double);
// --- determinant: ---
// par 1: InpVal, matr[16] (column matr)
double determinant44(double *);
// --- matrix inversion: ---
// par 1: InpVal, matr[16] (column matr)
// par 2: RetVal, matr[16] (column matr)
void InvMatr44(double *, double *);
// --- normalize a vector (ok also for quaternions): ---
// par 1: InpVal, vect [0],[1],[2],[3]
// par 2: RetVal, vect [0],[1],[2],[3]
void NormalizeVers4(double *, double *);
// ******* SECTION 4): *********************************************
// ******* Utility for vectors and matrices:
// ******* Vectors, points and matrices direct and inverse transf.
// ******* Print vectors or matrices on console (for debug).
// *****************************************************************
// --- direct transformation of a vector: (Vec3XtraspMatr33) ---
// par 1: ImpVal, vec [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------- cosines ----
// matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
#define transf_dir_vec33 Vec3XtraspMatr33
// --- inverse transformation of a vector: (Vec3XMatr33) ---
// par 1: ImpVal, vec [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------- cosines ----
// matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
//
// par 3: RetVal, vec [0],[1],[2]
#define transf_inv_vec33 Vec3XMatr33
// --- direct transformation of a point: (sub, Vec3XtraspMatr33) ---
// par 1: ImpVal, point [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_dir_pt331(double *, double *, double *);
// --- inverse transformation of a point: (Vec3XMatr33, add) ---
// par 1: ImpVal, point [0],[1],[2]
//
// par 2: InpVal, matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 3: RetVal, point [0],[1],[2]
void transf_inv_pt331(double *, double *, double *);
// --- direct transformation of a matrix: MatrXtraspMatr33,
// (sub, Vec3XtraspMatr33) ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: InpVal, matr[16]
// par 3: RetVal, matr[16]
void transf_dir_axs331(double *, double *, double *);
// --- inverse transformation of a matrix: MatrXMatr33,
// (Vec3XMatr33, add) ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: InpVal, matr[16]
// par 3: RetVal, matr[16]
void transf_inv_axs331(double *, double *, double *);
// --- Invert definition of an axis system: ---
// matrix declared matr[16]
// ------ cosines --|-- ori (xyz) --
// matr [0],[4],[8] , [12]
// [1],[5],[9] , [13]
// [2],[6],[10], [14]
//
// par 1: ImpVal, matr[16]
// par 2: RetVal, matr[16]
void inv_axs_def331(double *, double *);
// --- return identity matrix: ---
// par 1: InpVal, matrix declared matr[16]
// matr [0],[4],[8] ,[12]
// [1],[5],[9] ,[13]
// [2],[6],[10],[14]
// [3],[7],[11],[15]
//
// par 1: RetVal, 1 0 0 0
// 0 1 0 0
// 0 0 1 0
// 0 0 0 1
void MatrIdentify( double *);
// --- print on console (printf()) a matrix: ---
// --- mode(par1): 1) 3x3,col 2) 3x3,row 3) 4x4,col 4) 4x4,row
// matrix declared matr[16]
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, matr [0],[4],[8]
// [1],[5],[9]
// [2],[6],[10]
// If par 1 = 3 o par 1 = 4 par 2 will be all the matrix[16]
void print_matr(int, double *);
// --- print on console (printf()) a vector (3 o 4 elements)
// --- mode(par1): 1) vect3,col 2) vect3,row 3) vect4,col 4) vect4,row
// par 1: InpVal, var
// If par 1 = 1 o par 1 = 2:
// par 2: InpVal, vect [0],[1],[2]
// If par 1 = 3 o par 1 = 4 par 2 will be all vect[4]
void print_vers(int, double *);
// ******* SECTION 5): *********************************************
// ******* Geometry utilities (distance, intersection, ...):
// **************************************************************** */
// --- distance between 2 points xy(2D): ---
// par 1: InpVal, vect [0],[1]
// par 2: InpVal, vect [0],[1]
// par 3: RetVal, &var
void dist_pt2pt2(double *, double *, double *);
// --- distance between 2 points xyz(3D): ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: RetVal, &var
void dist_pt3pt3(double *, double *, double *);
// --- intersection of 2 circ(2D): 1° circ: x,y center and radius.
// --- 2° circ: x,y center and radius.
// --- Return in x1,y1 e x2,y2 ---
// par 1: InpVal, &var | par 1: RetVal, &var
// par 2: InpVal, &var | par 2: RetVal, &var
// par 3: InpVal, var
// par 4: InpVal, &var | par 4: RetVal, &var
// par 5: InpVal, &var | par 5: RetVal, &var
// par 6: InpVal, var
int c2c (double *, double *, double, double *, double *, double);
// --- triangle angles, if we know sides. ---
// par 1: InpVal, var
// par 2: InpVal, var
// par 3: InpVal, var
// par 4: RetVal, vect [0],[1],[2]
void tri(double, double, double, double *);
// --- 4 points on a sphere, returns center (xyz) and radius: ---
// par 1: InpVal, vect [0],[1],[2]
// par 2: InpVal, vect [0],[1],[2]
// par 3: InpVal, vect [0],[1],[2]
// par 4: InpVal, vect [0],[1],[2]
// par 5: RetVal, vect [0],[1],[2]
// par 6: RetVal, &var
int sfera(double *, double *, double *, double *, double *, double *);
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- distance point - line (3d). ---
// double line[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, line [0],[1],[2] --- start pt
// [4],[5],[6] --- end pt
// par 3: RetVal, var --- distance
// par 4: RetVal, vect [0],[1],[2] --- pt on the line
void dist_Point_to_Line( double *, double *, double *, double *);
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// get base of perpendicular from point to a plane
// Input: P = a 3D point
// PL = a plane with point V0 and normal n
// Output: *B(pt_on_pln) = base point on PL of perpendicular from P
// Return: the distance from P to the plane PL
//
// double plane[8];
// par 1: InpVal, point [0],[1],[2]
// par 2: InpVal, plane [0],[1],[2] x,y,z pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2]
// par 4: RetVal, var distance
void dist_Point_to_Plane( double *, double *, double *, double *);
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// --- Input: two 3D lines L1 and L2 --- Return: the shortest distance ---
// double line[8]; // [0],[1],[2] x,y,z start pt [4],[5],[6] end pt
// par 1: InpVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 2: InpVal, line [0],[1],[2]
// [4],[5],[6] end pt
// par 3: RetVal, &var
// par 4: RetVal, &var
// par 5: RetVal, point [0],[1],[2]
// par 6: RetVal, point [0],[1],[2]
// Return: the shortest distance between L1 and L2,
// lines parameters in vL1par and vL2par
// and nearest points in pt_1 and pt_2
double dist3D_Line_to_Line( double *, double *, double *, double *, double *, double *);
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_SegmentPlane(): intersect a segment and a plane
// Input: S = a segment, and Pn = a plane = {Point V0; Vector n;}
// Output: *I0 = the intersect point (when it exists)
// Return: 0 = disjoint (no intersection)
// 1 = intersection in the unique point *I0
// 2 = the segment lies in the plane
//
// double segm[8]; double plan[8]; double pnt[4]; int *retval;
// par 1: InpVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, point [0],[1],[2] inters. point
// par 4: RetVal, &var return flag
void inters_ln_pln( double *, double *, double *, int *);
// Copyright 2001, softSurfer (www.softsurfer.com)
// c porting rogx@libero.it 2012
//===================================================================
// intersect3D_2Planes(): the 3D intersect of two planes
// Input: two planes Pn1 and Pn2
// Output: *L = the intersection line (when it exists)
// Return: 0 = disjoint (no intersection)
// 1 = the two planes coincide
// 2 = intersection in the unique line *L
// par 1: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 2: InpVal, plane [0],[1],[2] pt on pln
// [4],[5],[6] vec normal to pln from pt
// par 3: RetVal, line [0],[1],[2] start pt
// [4],[5],[6] end pt
// par 4: RetVal, &var return flag
void inters_pln_pln(double *, double *, double *, int *);
// --- Geometric center of gravity of a triangle, knowing the vertices: ---
// par 1: InpVal, pt [0],[1],[2]
// par 2: InpVal, pt [0],[1],[2]
// par 3: RetVal, pt [0],[1],[2]
// par 4: RetVal, pt [0],[1],[2]
void tri_centroid(double *, double *, double *, double *);
// *****************************************************************
// *****************************************************************