Changement de repere 3D

Voici les sources d'un petit programme pour calculer les coordonnees
d'un point 3D d'un systeme de coordonnee a un autre:

// a: double[3]: angles de rotation du 2eme repere par rapport au premier
// t: double[3]: translation du 2eme repere par rapport au premier
// p: double[3]: coordonnees du point
// repereMonde: repere dans lequel les coordonnees du point sont
exprimees = true -> repere monde, false -> 2eme repere
// np: double[3]: coordonnees calculees (output)

void ccs( double *a, double *t, double *p, bool repereMonde, double *np )
{
// calcul la matrice de transformation
double mat[16];
double cax = cos ( a[0] );
double cay = cos ( a[1] );
double caz = cos ( a[2] );
double sax = sin ( a[0] );
double say = sin ( a[1] );
double saz = sin ( a[2] );

// matrice est calculee facon OpenGL..
mat[0] = caz * cay;
mat[1] = saz * cay;
mat[2] = -say;
mat[3] = 0;

mat[4] = caz*say*sax-saz*cax;
mat[5] = saz*say*sax+caz*cax;
mat[6] = cay*sax;
mat[7] = 0;

mat[8] = caz*say*cax+saz*sax;
mat[9] = saz*say*cax-caz*sax;
mat[10] = cay * cax;
mat[11] = 0;

mat[12] = t[0];
mat[13] = t[1];
mat[14] = t[2];
mat[15] = 1;

if( repereMonde ) {
double tnp[3];
tnp[0] = p[0]-mat[12];
tnp[1] = p[1]-mat[13];
tnp[2] = p[2]-mat[14];
np[0] = mat[0]*tnp[0] + mat[1]*tnp[1] + mat[2]*tnp[2];
np[1] = mat[4]*tnp[0] + mat[5]*tnp[1] + mat[6]*tnp[2];
np[2] = mat[8]*tnp[0] + mat[9]*tnp[1] + mat[10]*tnp[2];
} else {
np[0] = mat[0]*p[0] + mat[4]*p[1] + mat[8]*p[2] + mat[12];
np[1] = mat[1]*p[0] + mat[5]*p[1] + mat[9]*p[2] + mat[13];
np[2] = mat[2]*p[0] + mat[6]*p[1] + mat[10]*p[2] + mat[14];
}
}

Je n'ai fait que quelques tests basiques donc il n'est pas impossible
qu'il y ait des erreurs planquees.

Attention: Il y a des problemes de precision donc il vaut mieux eviter
de faire beaucoup de transformations a la suite.