1663 lines
36 KiB
C++
1663 lines
36 KiB
C++
//********************************************
|
|
// Fonctions vectorielles et matricielles
|
|
// destinées à la commande de M. Bouri
|
|
//
|
|
// Auteur: Patrick Helmer
|
|
// Adaptation: Marc Kunze
|
|
//
|
|
// Un vecteur ou une matrice débute
|
|
// par l'indice 0
|
|
//********************************************
|
|
|
|
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "MatriceVectLib.h"
|
|
|
|
|
|
//********************************************
|
|
// Fonctions algébriques
|
|
//********************************************
|
|
|
|
/**
|
|
* Fonction élevant un nombre au carré
|
|
*/
|
|
double Sqr(double value)
|
|
{
|
|
return(value*value);
|
|
}
|
|
|
|
|
|
//********************************************
|
|
// Fonctions d'impression vectorielles et
|
|
// matricielles
|
|
//********************************************
|
|
|
|
/**
|
|
* Fonction imprimant une matrice 3x3
|
|
*/
|
|
void PrintMat3(TypeMat3 A, char Name[4])
|
|
{
|
|
int i, j;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<3; i++)
|
|
{
|
|
for (j=0; j<3; j++)
|
|
printf(" %12.9f", A[i][j]);
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction imprimant une matrice homogène 4x4
|
|
*/
|
|
void PrintMatH4(TypeMatH4 A, char Name[4])
|
|
{
|
|
int i, j;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<4; i++)
|
|
{
|
|
for (j=0; j<4; j++)
|
|
printf(" %0.6f", A[i][j]);
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction imprimant une matrice 6x6
|
|
*/
|
|
void PrintMat6(TypeMat6 A, char Name[4])
|
|
{
|
|
int i, j;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<6; i++)
|
|
{
|
|
for (j=0; j<6; j++)
|
|
printf(" %10.2e", A[i][j]);
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction imprimant un vecteur 3x1
|
|
*/
|
|
void PrintVect3(TypeVect3 A, char Name[4])
|
|
{
|
|
int i;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<3; i++)
|
|
printf("%0.9f\n", A[i]);
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction imprimant un vecteur homogène 4x1
|
|
*/
|
|
void PrintVectH4(TypeVectH4 A, char Name[4])
|
|
{
|
|
int i;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<4; i++)
|
|
printf("%0.9f\n", A[i]);
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction imprimant un vecteur 6x1
|
|
*/
|
|
void PrintVect6(TypeVect6 A, char Name[4])
|
|
{
|
|
int i;
|
|
|
|
printf("#### %s #### \n", Name);
|
|
for (i=0; i<6; i++)
|
|
printf("%0.9f\n", A[i]);
|
|
}
|
|
|
|
|
|
//********************************************
|
|
// Fonctions de manipulations vect et mat
|
|
//********************************************
|
|
|
|
/**
|
|
* Fonction copiant un vecteur 3x1
|
|
*/
|
|
void CopyVect3(TypeVect3 Vi, TypeVect3 Vc)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
Vc[i]=Vi[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction copiant un vecteur homogène 4x1
|
|
*/
|
|
void CopyVectH4(TypeVectH4 Vi, TypeVectH4 Vc)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<4; i++)
|
|
Vc[i]=Vi[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction copiant un vecteur 6x1
|
|
*/
|
|
void CopyVect6(TypeVect6 Vi, TypeVect6 Vc)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<6; i++)
|
|
Vc[i]=Vi[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction copiant une matrice 3x3
|
|
*/
|
|
void CopyMat3(TypeMat3 Mi, TypeMat3 Mc)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
Mc[i][j]=Mi[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction copiant une matrice homogène 4x4
|
|
*/
|
|
void CopyMatH4(TypeMatH4 Mi, TypeMatH4 Mc)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
Mc[i][j]=Mi[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction copiant une matrice 6x6
|
|
*/
|
|
void CopyMat6(TypeMat6 Mi, TypeMat6 Mc)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
Mc[i][j]=Mi[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction transposant une matrice 3x3
|
|
*/
|
|
void TranspMat3(TypeMat3 A, TypeMat3 At)
|
|
{
|
|
int i, j;
|
|
TypeMat3 Atrep;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
Atrep[j][i]=A[i][j];
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
At[i][j]=Atrep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction transposant une matrice
|
|
* homogène 4x4 (transposée conventionnelle)
|
|
*/
|
|
void TranspMatH4(TypeMatH4 A, TypeMatH4 At)
|
|
{
|
|
int i, j;
|
|
TypeMatH4 Atrep;
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
Atrep[j][i]=A[i][j];
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
At[i][j]=Atrep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction transposant une matrice 6x6
|
|
*/
|
|
void TranspMat6(TypeMat6 A, TypeMat6 At)
|
|
{
|
|
int i, j;
|
|
TypeMat6 Atrep;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
Atrep[j][i]=A[i][j];
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
At[i][j]=Atrep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction inversant une matrice homogèe 4x4
|
|
* inverse au sens homogène !=conventionnel
|
|
*/
|
|
void InvMatH4(TypeMatH4 A, TypeMatH4 Ai)
|
|
{
|
|
TypeMat3 R;
|
|
TypeVect3 P;
|
|
|
|
ExtractRotMatH4(A, R);
|
|
TranspMat3(R, R);
|
|
ExtractPosMatH4(A, P);
|
|
MulMat3Vect3(R, P, P);
|
|
MulVect3Scal(P, -1.0f, P);
|
|
MakeMatH4RotPos(R, P, Ai);
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant le produit scalaire
|
|
* de deux vecteurs 3x1
|
|
*/
|
|
double ProdScal3(TypeVect3 A, TypeVect3 B)
|
|
{
|
|
int i;
|
|
double r;
|
|
|
|
r=0;
|
|
for (i=0; i<3; i++)
|
|
r=r+A[i]*B[i];
|
|
return r;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant le produit scalaire
|
|
* de deux vecteurs homogènes 4x1
|
|
*/
|
|
double ProdScalH4(TypeVectH4 A, TypeVectH4 B)
|
|
{
|
|
int i;
|
|
double r;
|
|
|
|
r=0;
|
|
for (i=0; i<3; i++)
|
|
r=r+A[i]*B[i];
|
|
return r;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant le produit scalaire
|
|
* de deux vecteurs 6x1
|
|
*/
|
|
double ProdScal6(TypeVect6 A, TypeVect6 B)
|
|
{
|
|
int i;
|
|
double r;
|
|
|
|
r=0;
|
|
for (i=0; i<6; i++)
|
|
r=r+A[i]*B[i];
|
|
return r;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur 3x1
|
|
* par un scalaire
|
|
*/
|
|
void MulVect3Scal(TypeVect3 A, double coeff, TypeVect3 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]*coeff;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur
|
|
* homogène 4x1 par un scalaire
|
|
*/
|
|
void MulVectH4Scal(TypeVectH4 A, double coeff, TypeVectH4 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]*coeff;
|
|
R[3]=1;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur 6x1
|
|
* par un scalaire
|
|
*/
|
|
void MulVect6Scal(TypeVect6 A, double coeff, TypeVect6 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<6; i++)
|
|
R[i]=A[i]*coeff;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant une matrice 3x3
|
|
* par un scalaire
|
|
*/
|
|
void MulMat3Scal(TypeMat3 M, double coeff, TypeMat3 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
R[i][j]=M[i][j]*coeff;
|
|
}
|
|
|
|
/**
|
|
* Fonction multipliant une matrice 6x6
|
|
* par un scalaire
|
|
*/
|
|
void MulMat6Scal(TypeMat6 M, double coeff, TypeMat6 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
R[i][j]=M[i][j]*coeff;
|
|
}
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur 3x1
|
|
* par une matrice 3x3
|
|
*/
|
|
void MulMat3Vect3(TypeMat3 M, TypeVect3 A, TypeVect3 R)
|
|
{
|
|
int i, j;
|
|
TypeVect3 Rep;
|
|
|
|
for (i=0; i<3; i++)
|
|
{
|
|
Rep[i]=0;
|
|
for (j=0; j<3; j++)
|
|
Rep[i]=Rep[i]+M[i][j]*A[j];
|
|
}
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=Rep[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur 6x1
|
|
* par une matrice 6x6
|
|
*/
|
|
void MulMat6Vect6(TypeMat6 M, TypeVect6 A, TypeVect6 R)
|
|
{
|
|
int i, j;
|
|
TypeVect6 Rep;
|
|
|
|
for (i=0; i<6; i++)
|
|
{
|
|
Rep[i]=0;
|
|
for (j=0; j<6; j++)
|
|
Rep[i]=Rep[i]+M[i][j]*A[j];
|
|
}
|
|
|
|
for (i=0; i<6; i++)
|
|
R[i]=Rep[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur 3x1
|
|
* par une matrice homogène 4x4
|
|
*/
|
|
void MulMatH4Vect3(TypeMatH4 M, TypeVect3 A, TypeVect3 R)
|
|
{
|
|
int i, j;
|
|
TypeVect3 Rep;
|
|
|
|
for (i=0; i<3; i++)
|
|
{
|
|
Rep[i]=0;
|
|
for (j=0; j<3; j++)
|
|
Rep[i]=Rep[i]+M[i][j]*A[j];
|
|
Rep[i]=Rep[i]+M[i][3];
|
|
}
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=Rep[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant un vecteur
|
|
* homogène 4x1 par une matrice homogène 4x4
|
|
*/
|
|
void MulMatH4VectH4(TypeMatH4 M, TypeVectH4 A, TypeVectH4 R)
|
|
{
|
|
int i, j;
|
|
TypeVectH4 Rep;
|
|
|
|
for (i=0; i<4; i++)
|
|
{
|
|
Rep[i]=0;
|
|
for (j=0; j<4; j++)
|
|
Rep[i]=Rep[i]+M[i][j]*A[j];
|
|
}
|
|
|
|
for (i=0; i<4; i++)
|
|
R[i]=Rep[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant une matrice 3x3
|
|
* par une matrice 3x3
|
|
*/
|
|
void MulMatMat3(TypeMat3 M1, TypeMat3 M2, TypeMat3 R)
|
|
{
|
|
int i, j, k;
|
|
TypeMat3 Rep;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
{
|
|
Rep[i][j]=0;
|
|
for (k=0; k<3; k++)
|
|
Rep[i][j]=Rep[i][j]+M1[i][k]*M2[k][j];
|
|
}
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
R[i][j]=Rep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant une matrice
|
|
* homogène 4x4 par une matrice homogène 4x4
|
|
*/
|
|
void MulMatMatH4(TypeMatH4 M1, TypeMatH4 M2, TypeMatH4 R)
|
|
{
|
|
int i, j, k;
|
|
TypeMatH4 Rep;
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
{
|
|
Rep[i][j]=0;
|
|
for (k=0; k<4; k++)
|
|
Rep[i][j]=Rep[i][j]+M1[i][k]*M2[k][j];
|
|
}
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
R[i][j]=Rep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction multipliant une matrice 6x6
|
|
* par une matrice 6x6
|
|
*/
|
|
void MulMatMat6(TypeMat6 M1, TypeMat6 M2, TypeMat6 R)
|
|
{
|
|
int i, j, k;
|
|
TypeMat6 Rep;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
{
|
|
Rep[i][j]=0;
|
|
for (k=0; k<6; k++)
|
|
Rep[i][j]=Rep[i][j]+M1[i][k]*M2[k][j];
|
|
}
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
R[i][j]=Rep[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction sommant deux vecteurs 3x1
|
|
*/
|
|
void SomVectVect3(TypeVect3 A,TypeVect3 B, TypeVect3 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]+B[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction soustrayant deux vecteurs 3x1
|
|
*/
|
|
void SubVectVect3(TypeVect3 A, TypeVect3 B, TypeVect3 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]-B[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction sommant deux vecteurs
|
|
* homogènes 4x1
|
|
*/
|
|
void SomVectVectH4(TypeVectH4 A, TypeVectH4 B, TypeVectH4 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]+B[i];
|
|
R[3]=1;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction soustrayant deux vecteurs
|
|
* homogènes 4x1
|
|
*/
|
|
void SubVectVectH4(TypeVectH4 A, TypeVectH4 B, TypeVectH4 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<3; i++)
|
|
R[i]=A[i]-B[i];
|
|
R[3]=1;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction sommant deux vecteurs 6x1
|
|
*/
|
|
void SomVectVect6(TypeVect6 A, TypeVect6 B, TypeVect6 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<6; i++)
|
|
R[i]=A[i]+B[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction soustrayant deux vecteurs 6x1
|
|
*/
|
|
void SubVectVect6(TypeVect6 A, TypeVect6 B, TypeVect6 R)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i<6; i++)
|
|
R[i]=A[i]-B[i];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction sommant deux matrices 3x3
|
|
*/
|
|
void SomMatMat3(TypeMat3 A, TypeMat3 B, TypeMat3 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
R[i][j]=A[i][j]+B[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction soustrayant deux matrices 3x3
|
|
*/
|
|
void SubMatMat3(TypeMat3 A, TypeMat3 B, TypeMat3 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
R[i][j]=A[i][j]-B[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction sommant deux matrices 6x6
|
|
*/
|
|
void SomMatMat6(TypeMat6 A, TypeMat6 B, TypeMat6 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
R[i][j]=A[i][j]+B[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction soustrayant deux matrices 6x6
|
|
*/
|
|
void SubMatMat6(TypeMat6 A, TypeMat6 B, TypeMat6 R)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
R[i][j]=A[i][j]-B[i][j];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction initialisant un vecteur 3x1
|
|
*/
|
|
void InitialVect3(TypeVect3 A)
|
|
{
|
|
A[0]=0.0f;
|
|
A[1]=0.0f;
|
|
A[2]=0.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction initialisant un vecteur
|
|
* homogène 4x1
|
|
*/
|
|
void InitialVectH4(TypeVectH4 A)
|
|
{
|
|
A[0]=0.0f;
|
|
A[1]=0.0f;
|
|
A[2]=0.0f;
|
|
A[3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction initialisant un vecteur 6x1
|
|
*/
|
|
void InitialVect6(TypeVect6 A)
|
|
{
|
|
A[0]=0.0f;
|
|
A[1]=0.0f;
|
|
A[2]=0.0f;
|
|
A[3]=0.0f;
|
|
A[4]=0.0f;
|
|
A[5]=0.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction initialisant une matrice 3x3
|
|
*/
|
|
void InitialMat3(TypeMat3 A)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
A[i][j]=0.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction initialisant une matrice 6x6
|
|
*/
|
|
void InitialMat6(TypeMat6 A)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
A[i][j]=0.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice identité 3x3
|
|
*/
|
|
void IdMat3(TypeMat3 A)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
A[i][j]=0.0f;
|
|
A[0][0]=1.0f;
|
|
A[1][1]=1.0f;
|
|
A[2][2]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice
|
|
* homogène identité 4x4
|
|
*/
|
|
void IdMatH4(TypeMatH4 A)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
A[i][j]=0.0f;
|
|
A[0][0]=1.0f;
|
|
A[1][1]=1.0f;
|
|
A[2][2]=1.0f;
|
|
A[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice identité 6x6
|
|
*/
|
|
void IdMat6(TypeMat6 A)
|
|
{
|
|
int i, j;
|
|
|
|
for (i=0; i<6; i++)
|
|
for (j=0; j<6; j++)
|
|
A[i][j]=0.0f;
|
|
A[0][0]=1.0f;
|
|
A[1][1]=1.0f;
|
|
A[2][2]=1.0f;
|
|
A[3][3]=1.0f;
|
|
A[4][4]=1.0f;
|
|
A[5][5]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant la norme au carré
|
|
* d'un vecteur 3x1
|
|
*/
|
|
double NormeCarreVect3(TypeVect3 A)
|
|
{
|
|
int i;
|
|
double norme=0.0f;
|
|
|
|
for (i=0; i<3; i++)
|
|
norme=norme+A[i]*A[i];
|
|
return norme;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant la norme au carré
|
|
* d'un vecteur homogène 4x1
|
|
*/
|
|
double NormeCarreVectH4(TypeVectH4 A)
|
|
{
|
|
int i;
|
|
double norme=0.0f;
|
|
|
|
for (i=0; i<3; i++)
|
|
norme=norme+A[i]*A[i];
|
|
return norme;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant un vecteur homogène 4x1
|
|
* à partir d'un vecteur position 3x1
|
|
*/
|
|
void MakeVectH4Pos(TypeVect3 Pos, TypeVectH4 V)
|
|
{
|
|
V[0]=Pos[0];
|
|
V[1]=Pos[1];
|
|
V[2]=Pos[2];
|
|
V[3]=1;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice de rotation 3x3
|
|
* à partir d'un vecteur de position
|
|
* angulaire 3x1
|
|
*/
|
|
void MakeMat3Rot(TypeVect3 Ang, TypeMat3 R)
|
|
{
|
|
double sinalpha=sin(Ang[0]);
|
|
double cosalpha=cos(Ang[0]);
|
|
double sinbeta=sin(Ang[1]);
|
|
double cosbeta=cos(Ang[1]);
|
|
double singamma=sin(Ang[2]);
|
|
double cosgamma=cos(Ang[2]);
|
|
|
|
R[0][0]=cosbeta*cosgamma;
|
|
R[0][1]=cosgamma*sinalpha*sinbeta - cosalpha*singamma;
|
|
R[0][2]=cosalpha*cosgamma*sinbeta + sinalpha*singamma;
|
|
R[1][0]=cosbeta*singamma;
|
|
R[1][1]=cosalpha*cosgamma + sinalpha*sinbeta*singamma;
|
|
R[1][2]=-cosgamma*sinalpha + cosalpha*sinbeta*singamma;
|
|
R[2][0]=-sinbeta;
|
|
R[2][1]=cosbeta*sinalpha;
|
|
R[2][2]=cosalpha*cosbeta;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice homogène 4x4
|
|
* à partir d'un vecteur position angulaire
|
|
* 3x1 et d'un vecteur de position 3x1
|
|
*/
|
|
void MakeMatH4AngPos(TypeVect3 Ang, TypeVect3 Pos, TypeMatH4 H)
|
|
{
|
|
double sinalpha=sin(Ang[0]);
|
|
double cosalpha=cos(Ang[0]);
|
|
double sinbeta=sin(Ang[1]);
|
|
double cosbeta=cos(Ang[1]);
|
|
double singamma=sin(Ang[2]);
|
|
double cosgamma=cos(Ang[2]);
|
|
|
|
H[0][0]=cosbeta*cosgamma;
|
|
H[0][1]=cosgamma*sinalpha*sinbeta - cosalpha*singamma;
|
|
H[0][2]=cosalpha*cosgamma*sinbeta + sinalpha*singamma;
|
|
H[0][3]=Pos[0];
|
|
H[1][0]=cosbeta*singamma;
|
|
H[1][1]=cosalpha*cosgamma + sinalpha*sinbeta*singamma;
|
|
H[1][2]=-cosgamma*sinalpha + cosalpha*sinbeta*singamma;
|
|
H[1][3]=Pos[1];
|
|
H[2][0]=-sinbeta;
|
|
H[2][1]=cosbeta*sinalpha;
|
|
H[2][2]=cosalpha*cosbeta;
|
|
H[2][3]=Pos[2];
|
|
H[3][0]=0.0f;
|
|
H[3][1]=0.0f;
|
|
H[3][2]=0.0f;
|
|
H[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice homogène 4x4
|
|
* à partir d'une matrice de rotation 3x3
|
|
* et d'un vecteur de position 3x1
|
|
*/
|
|
void MakeMatH4RotPos(TypeMat3 R, TypeVect3 Pos, TypeMatH4 H)
|
|
{
|
|
H[0][0]=R[0][0];
|
|
H[0][1]=R[0][1];
|
|
H[0][2]=R[0][2];
|
|
H[0][3]=Pos[0];
|
|
H[1][0]=R[1][0];
|
|
H[1][1]=R[1][1];
|
|
H[1][2]=R[1][2];
|
|
H[1][3]=Pos[1];
|
|
H[2][0]=R[2][0];
|
|
H[2][1]=R[2][1];
|
|
H[2][2]=R[2][2];
|
|
H[2][3]=Pos[2];
|
|
H[3][0]=0.0f;
|
|
H[3][1]=0.0f;
|
|
H[3][2]=0.0f;
|
|
H[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice homogène 4x4
|
|
* à partir d'un vecteur de position angulaire
|
|
* inverse 3x1
|
|
* et d'un vecteur de position 3x1
|
|
*/
|
|
void MakeMatH4InvAngPos(TypeVect3 Ang, TypeVect3 Pos, TypeMatH4 H)
|
|
{
|
|
double sinalpha=sin(Ang[0]);
|
|
double cosalpha=cos(Ang[0]);
|
|
double sinbeta=sin(Ang[1]);
|
|
double cosbeta=cos(Ang[1]);
|
|
double singamma=sin(Ang[2]);
|
|
double cosgamma=cos(Ang[2]);
|
|
|
|
H[0][0]=cosbeta*cosgamma;
|
|
H[0][1]=cosbeta*singamma;
|
|
H[0][2]=-sinbeta;
|
|
H[0][3]=-Pos[0]*cosbeta*cosgamma + Pos[2]*sinbeta - Pos[1]*cosbeta*singamma;
|
|
H[1][0]=cosgamma*sinalpha*sinbeta - cosalpha*singamma;
|
|
H[1][1]=cosalpha*cosgamma + sinalpha*sinbeta*singamma;
|
|
H[1][2]=cosbeta*sinalpha;
|
|
H[1][3]=-Pos[2]*cosbeta*sinalpha - Pos[0]*(cosgamma*sinalpha*sinbeta - cosalpha*singamma)
|
|
- Pos[1]*(cosalpha*cosgamma + sinalpha*sinbeta*singamma);
|
|
H[2][0]=cosalpha*cosgamma*sinbeta + sinalpha*singamma;
|
|
H[2][1]=-cosgamma*sinalpha + cosalpha*sinbeta*singamma;
|
|
H[2][2]=cosalpha*cosbeta;
|
|
H[2][3]=-Pos[2]*cosalpha*cosbeta - Pos[0]*(cosalpha*cosgamma*sinbeta + sinalpha*singamma)
|
|
- Pos[1]*(-cosgamma*sinalpha + cosalpha*sinbeta*singamma);
|
|
H[3][0]=0.0f;
|
|
H[3][1]=0.0f;
|
|
H[3][2]=0.0f;
|
|
H[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice homogène 4x4
|
|
* à partir d'une matrice de rotation inverse
|
|
* 3x3 et d'un vecteur de position 3x1
|
|
*/
|
|
void MakeMatH4InvRotPos(TypeMat3 R, TypeVect3 Pos, TypeMatH4 H)
|
|
{
|
|
H[0][0]=R[0][0];
|
|
H[0][1]=R[1][0];
|
|
H[0][2]=R[2][0];
|
|
H[0][3]=-Pos[0]*R[0][0] - Pos[1]*R[1][0] - Pos[2]*R[2][0];
|
|
H[1][0]=R[0][1];
|
|
H[1][1]=R[1][1];
|
|
H[1][2]=R[2][1];
|
|
H[1][3]=-Pos[0]*R[0][1] - Pos[1]*R[1][1] - Pos[2]*R[2][1] ;
|
|
H[2][0]=R[0][2];
|
|
H[2][1]=R[1][2];
|
|
H[2][2]=R[2][2];
|
|
H[2][3]=-Pos[0]*R[0][2] - Pos[1]*R[1][2] - Pos[2]*R[2][2] ;
|
|
H[3][0]=0.0f;
|
|
H[3][1]=0.0f;
|
|
H[3][2]=0.0f;
|
|
H[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice homogène 4x4
|
|
* à partir d'un vecteur de
|
|
* position généralisée 6x1
|
|
*/
|
|
void MakeMatH4PosGen(TypeVect6 Pos,TypeMatH4 H)
|
|
{
|
|
double sinalpha=sin(Pos[alpha]);
|
|
double cosalpha=cos(Pos[alpha]);
|
|
double sinbeta=sin(Pos[beta]);
|
|
double cosbeta=cos(Pos[beta]);
|
|
double singamma=sin(Pos[gamma]);
|
|
double cosgamma=cos(Pos[gamma]);
|
|
|
|
H[0][0]=cosbeta*cosgamma;
|
|
H[0][1]=cosgamma*sinalpha*sinbeta - cosalpha*singamma;
|
|
H[0][2]=cosalpha*cosgamma*sinbeta + sinalpha*singamma;
|
|
H[0][3]=Pos[0];
|
|
H[1][0]=cosbeta*singamma;
|
|
H[1][1]=cosalpha*cosgamma + sinalpha*sinbeta*singamma;
|
|
H[1][2]=-cosgamma*sinalpha + cosalpha*sinbeta*singamma;
|
|
H[1][3]=Pos[1];
|
|
H[2][0]=-sinbeta;
|
|
H[2][1]=cosbeta*sinalpha;
|
|
H[2][2]=cosalpha*cosbeta;
|
|
H[2][3]=Pos[2];
|
|
H[3][0]=0.0f;
|
|
H[3][1]=0.0f;
|
|
H[3][2]=0.0f;
|
|
H[3][3]=1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice 6x6 liant
|
|
* la vitesse d'un point C attaché au point B
|
|
* à la vitesse du point B,
|
|
* le tout exprimé dans un référentiel A
|
|
*/
|
|
void MakeMat6RotPVit(TypeMat3 R,TypeVect6 Pos, TypeMat6 O)
|
|
{
|
|
O[0][0] = 1.0f;
|
|
O[0][1] = 0.0f;
|
|
O[0][2] = 0.0f;
|
|
O[0][3] = 0.0f;
|
|
O[0][4] = R[2][0]*Pos[0] + R[2][1]*Pos[1] + R[2][2]*Pos[2];
|
|
O[0][5] = -R[1][0]*Pos[0] - R[1][1]*Pos[1] - R[1][2]*Pos[2];
|
|
O[1][0] = 0.0f;
|
|
O[1][1] = 1.0f;
|
|
O[1][2] = 0.0f;
|
|
O[1][3] = -R[2][0]*Pos[0] - R[2][1]*Pos[1] - R[2][2]*Pos[2];
|
|
O[1][4] = 0.0f;
|
|
O[1][5] = R[0][0]*Pos[0] + R[0][1]*Pos[1] + R[0][2]*Pos[2];
|
|
O[2][0] = 0.0f;
|
|
O[2][1] = 0.0f;
|
|
O[2][2] = 1.0f;
|
|
O[2][3] = R[1][0]*Pos[0] + R[1][1]*Pos[1] + R[1][2]*Pos[2];
|
|
O[2][4] = -R[0][0]*Pos[0] - R[0][1]*Pos[1] - R[0][2]*Pos[2];
|
|
O[2][5] = 0.0f;
|
|
O[3][0] = 0.0f;
|
|
O[3][1] = 0.0f;
|
|
O[3][2] = 0.0f;
|
|
O[3][3] = 1.0f;
|
|
O[3][4] = 0.0f;
|
|
O[3][5] = 0.0f;
|
|
O[4][0] = 0.0f;
|
|
O[4][1] = 0.0f;
|
|
O[4][2] = 0.0f;
|
|
O[4][3] = 0.0f;
|
|
O[4][4] = 1.0f;
|
|
O[4][5] = 0.0f;
|
|
O[5][0] = 0.0f;
|
|
O[5][1] = 0.0f;
|
|
O[5][2] = 0.0f;
|
|
O[5][3] = 0.0f;
|
|
O[5][4] = 0.0f;
|
|
O[5][5] = 1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice liant
|
|
* la force en un point C à la force
|
|
* équivalente en un point B,
|
|
* le tout exprimé dans un référentiel A
|
|
*/
|
|
void MakeMat6RotPForce(TypeMat3 R,TypeVect6 Pos, TypeMat6 O)
|
|
{
|
|
O[0][0] = 1.0f;
|
|
O[0][1] = 0.0f;
|
|
O[0][2] = 0.0f;
|
|
O[0][3] = 0.0f;
|
|
O[0][4] = 0.0f;
|
|
O[0][5] = 0.0f;
|
|
O[1][0] = 0.0f;
|
|
O[1][1] = 1.0f;
|
|
O[1][2] = 0.0f;
|
|
O[1][3] = 0.0f;
|
|
O[1][4] = 0.0f;
|
|
O[1][5] = 0.0f;
|
|
O[2][0] = 0.0f;
|
|
O[2][1] = 0.0f;
|
|
O[2][2] = 1.0f;
|
|
O[2][3] = 0.0f;
|
|
O[2][4] = 0.0f;
|
|
O[2][5] = 0.0f;
|
|
O[3][0] = 0.0f;
|
|
O[3][1] = -R[2][0]*Pos[0] - R[2][1]*Pos[1] - R[2][2]*Pos[2];
|
|
O[3][2] = R[1][0]*Pos[0] + R[1][1]*Pos[1] + R[1][2]*Pos[2];
|
|
O[3][3] = 1.0f;
|
|
O[3][4] = 0.0f;
|
|
O[3][5] = 0.0f;
|
|
O[4][0] = R[2][0]*Pos[0] + R[2][1]*Pos[1] + R[2][2]*Pos[2];
|
|
O[4][1] = 0.0f;
|
|
O[4][2] = -R[0][0]*Pos[0] - R[0][1]*Pos[1] - R[0][2]*Pos[2];
|
|
O[4][3] = 0.0f;
|
|
O[4][4] = 1.0f;
|
|
O[4][5] = 0.0f;
|
|
O[5][0] = -R[1][0]*Pos[0] - R[1][1]*Pos[1] - R[1][2]*Pos[2];
|
|
O[5][1] = R[0][0]*Pos[0] + R[0][1]*Pos[1] + R[0][2]*Pos[2];
|
|
O[5][2] = 0.0f;
|
|
O[5][3] = 0.0f;
|
|
O[5][4] = 0.0f;
|
|
O[5][5] = 1.0f;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice 6x6 contenant
|
|
* deux matrices de rotation 3x3
|
|
* sur la diagonale
|
|
*/
|
|
void MakeMat6Rot(TypeMat3 R,TypeMat6 O)
|
|
{
|
|
O[0][0] = R[0][0];
|
|
O[0][1] = R[0][1];
|
|
O[0][2] = R[0][2];
|
|
O[0][3] = 0.0f;
|
|
O[0][4] = 0.0f;
|
|
O[0][5] = 0.0f;
|
|
O[1][0] = R[1][0];
|
|
O[1][1] = R[1][1];
|
|
O[1][2] = R[1][2];
|
|
O[1][3] = 0.0f;
|
|
O[1][4] = 0.0f;
|
|
O[1][5] = 0.0f;
|
|
O[2][0] = R[2][0];
|
|
O[2][1] = R[2][1];
|
|
O[2][2] = R[2][2];
|
|
O[2][3] = 0.0f;
|
|
O[2][4] = 0.0f;
|
|
O[2][5] = 0.0f;
|
|
O[3][0] = 0.0f;
|
|
O[3][1] = 0.0f;
|
|
O[3][2] = 0.0f;
|
|
O[3][3] = R[0][0];
|
|
O[3][4] = R[0][1];
|
|
O[3][5] = R[0][2];
|
|
O[4][0] = 0.0f;
|
|
O[4][1] = 0.0f;
|
|
O[4][2] = 0.0f;
|
|
O[4][3] = R[1][0];
|
|
O[4][4] = R[1][1];
|
|
O[4][5] = R[1][2];
|
|
O[5][0] = 0.0f;
|
|
O[5][1] = 0.0f;
|
|
O[5][2] = 0.0f;
|
|
O[5][3] = R[2][0];
|
|
O[5][4] = R[2][1];
|
|
O[5][5] = R[2][2];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant une matrice de gain 6x6
|
|
* ayant comme diagonale les valeurs
|
|
* fournies en entrée
|
|
*/
|
|
void MakeMat6Gain(double gain_1,double gain_2,double gain_3,double gain_4,double gain_5,double gain_6,TypeMat6 R)
|
|
{
|
|
R[0][0] = gain_1;
|
|
R[0][1] = 0.0f;
|
|
R[0][2] = 0.0f;
|
|
R[0][3] = 0.0f;
|
|
R[0][4] = 0.0f;
|
|
R[0][5] = 0.0f;
|
|
R[1][0] = 0.0f;
|
|
R[1][1] = gain_2;
|
|
R[1][2] = 0.0f;
|
|
R[1][3] = 0.0f;
|
|
R[1][4] = 0.0f;
|
|
R[1][5] = 0.0f;
|
|
R[2][0] = 0.0f;
|
|
R[2][1] = 0.0f;
|
|
R[2][2] = gain_3;
|
|
R[2][3] = 0.0f;
|
|
R[2][4] = 0.0f;
|
|
R[2][5] = 0.0f;
|
|
R[3][0] = 0.0f;
|
|
R[3][1] = 0.0f;
|
|
R[3][2] = 0.0f;
|
|
R[3][3] = gain_4;
|
|
R[3][4] = 0.0f;
|
|
R[3][5] = 0.0f;
|
|
R[4][0] = 0.0f;
|
|
R[4][1] = 0.0f;
|
|
R[4][2] = 0.0f;
|
|
R[4][3] = 0.0f;
|
|
R[4][4] = gain_5;
|
|
R[4][5] = 0.0f;
|
|
R[5][0] = 0.0f;
|
|
R[5][1] = 0.0f;
|
|
R[5][2] = 0.0f;
|
|
R[5][3] = 0.0f;
|
|
R[5][4] = 0.0f;
|
|
R[5][5] = gain_6;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction créant un vecteur de gain 6x1
|
|
* à partir des valeurs fournies en entrée
|
|
*/
|
|
void MakeVect6Gain(double gain_1,double gain_2,double gain_3,double gain_4,double gain_5,double gain_6,TypeVect6 R)
|
|
{
|
|
R[0]= gain_1;
|
|
R[1]= gain_2;
|
|
R[2]= gain_3;
|
|
R[3]= gain_4;
|
|
R[4]= gain_5;
|
|
R[5]= gain_6;
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la position
|
|
* angulaire dans un vecteur 3x1
|
|
* à partir d'une matrice de rotation 3x3
|
|
*/
|
|
void ExtractAngMat3(TypeMat3 R, TypeVect3 Ang)
|
|
// ne fonctionne correctement que pour -90°<beta<90°, cos^2(beta) seul accessible
|
|
// si cos(beta) prévu < 0, alors ajouter signe "-" devant sqrt
|
|
{
|
|
double cosbeta;
|
|
|
|
Ang[1]=atan2(-R[2][0], sqrt(R[0][0]*R[0][0]+R[1][0]*R[1][0]));
|
|
cosbeta=cos(Ang[1]);
|
|
if (fabs(cosbeta)>=EPSILON_DIV_ZERO_DOUBLE)
|
|
{
|
|
Ang[0]=atan2(R[2][1]/cosbeta, R[2][2]/cosbeta);
|
|
Ang[2]=atan2(R[1][0]/cosbeta, R[0][0]/cosbeta);
|
|
}
|
|
else
|
|
{
|
|
if (R[2][0]<=0)
|
|
{
|
|
Ang[2]=0.0;
|
|
Ang[0]=atan2(R[0][1], R[1][1]);
|
|
}
|
|
else
|
|
{
|
|
Ang[2]=0.0;
|
|
Ang[0]=-atan2(R[0][1], R[1][1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la position
|
|
* angulaire dans un vecteur 3x1
|
|
* à partir d'une matrice homogène 4x4
|
|
*/
|
|
void ExtractAngMatH4(TypeMatH4 H, TypeVect3 Ang)
|
|
// ne fonctionne correctement que pour -90°<beta<90°, cos^2(beta) seul accessible
|
|
// si cos(beta) prévu < 0, alors ajouter signe "-" devant sqrt
|
|
{
|
|
double cosbeta;
|
|
|
|
Ang[1]=atan2(-H[2][0], sqrt(H[0][0]*H[0][0]+H[1][0]*H[1][0]));
|
|
cosbeta=cos(Ang[1]);
|
|
if (fabs(cosbeta)>=EPSILON_DIV_ZERO_DOUBLE)
|
|
{
|
|
Ang[0]=atan2(H[2][1]/cosbeta, H[2][2]/cosbeta);
|
|
Ang[2]=atan2(H[1][0]/cosbeta, H[0][0]/cosbeta);
|
|
}
|
|
else
|
|
{
|
|
if (H[2][0]<=0)
|
|
{
|
|
Ang[2]=0.0;
|
|
Ang[0]=atan2(H[0][1], H[1][1]);
|
|
}
|
|
else
|
|
{
|
|
Ang[2]=0.0;
|
|
Ang[0]=-atan2(H[0][1], H[1][1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la position
|
|
* dans un vecteur 3x1
|
|
* à partir d'un vecteur homogène 4x1
|
|
*/
|
|
void ExtractPosVectH4(TypeVectH4 V, TypeVect3 Pos)
|
|
{
|
|
Pos[axeX]=V[0];
|
|
Pos[axeY]=V[1];
|
|
Pos[axeZ]=V[2];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la position
|
|
* dans un vecteur 3x1
|
|
* à partir d'une matrice homogène 4x4
|
|
*/
|
|
void ExtractPosMatH4(TypeMatH4 H, TypeVect3 Pos)
|
|
{
|
|
Pos[axeX]=H[0][3];
|
|
Pos[axeY]=H[1][3];
|
|
Pos[axeZ]=H[2][3];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la matrice
|
|
* de rotation 3x3
|
|
* à partir d'une matrice homogène 4x4
|
|
*/
|
|
void ExtractRotMatH4(TypeMatH4 H, TypeMat3 R)
|
|
{
|
|
R[0][0]=H[0][0];
|
|
R[0][1]=H[0][1];
|
|
R[0][2]=H[0][2];
|
|
R[1][0]=H[1][0];
|
|
R[1][1]=H[1][1];
|
|
R[1][2]=H[1][2];
|
|
R[2][0]=H[2][0];
|
|
R[2][1]=H[2][1];
|
|
R[2][2]=H[2][2];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction extrayant la position
|
|
* généralisée dans un vecteur 6x1
|
|
* à partir d'une matrice homogène 4x4
|
|
*/
|
|
void ExtractPosGenMatH4(TypeMatH4 H,TypeVect6 Pos)
|
|
// ne fonctionne correctement que pour -90°<beta<90°, cos^2(beta) seul accessible
|
|
// si cos(beta) prévu < 0, alors ajouter signe "-" devant sqrt
|
|
{
|
|
double cosbeta;
|
|
|
|
Pos[beta]=atan2(-H[2][0], sqrt(H[0][0]*H[0][0]+H[1][0]*H[1][0]));
|
|
cosbeta=cos(Pos[beta]);
|
|
if (fabs(cosbeta)>=EPSILON_DIV_ZERO_DOUBLE)
|
|
{
|
|
Pos[alpha]=atan2(H[2][1]/cosbeta, H[2][2]/cosbeta);
|
|
Pos[gamma]=atan2(H[1][0]/cosbeta, H[0][0]/cosbeta);
|
|
}
|
|
else
|
|
{
|
|
if (H[2][0]<=0)
|
|
{
|
|
Pos[gamma]=0.0;
|
|
Pos[alpha]=atan2(H[0][1], H[1][1]);
|
|
}
|
|
else
|
|
{
|
|
Pos[gamma]=0.0;
|
|
Pos[alpha]=-atan2(H[0][1], H[1][1]);
|
|
}
|
|
}
|
|
Pos[axeX]=H[0][3];
|
|
Pos[axeY]=H[1][3];
|
|
Pos[axeZ]=H[2][3];
|
|
}
|
|
|
|
|
|
/**
|
|
* Fonction calculant l'inverse d'une matrice
|
|
* 6x6. Le résultat est dans la matrice A.
|
|
*/
|
|
void InvMat6(TypeMat6 A, TypeMat6 B)
|
|
// prend 46us sur pentium 500MHz n=6, m=1
|
|
{
|
|
const int n=6, m=1; // n=rang de A et m=nb de vect col de B à évaluer
|
|
int i, icol=0, irow=0, j, k, l, ll;
|
|
double big, dum, pivinv, temp;
|
|
int Indxc[n], Indxr[n], Ipiv[n];
|
|
|
|
|
|
for (j=0; j<n; j++)
|
|
Ipiv[j]=0;
|
|
for (i=0; i<n; i++) // boucle principale sur les col à réduire
|
|
{
|
|
big=0.0;
|
|
for (j=0; j<n; j++) // boucle extérieure de recherche d'un élément pivot
|
|
if (Ipiv[j]!=1)
|
|
for (k=0; k<n; k++)
|
|
{
|
|
if (Ipiv[k]==0)
|
|
{
|
|
if (fabs(A[j][k])>=big)
|
|
{
|
|
big=fabs(A[j][k]);
|
|
irow=j;
|
|
icol=k;
|
|
}
|
|
}
|
|
else
|
|
if (Ipiv[k]>1)
|
|
printf("Erreur d'inversion pt 1: matrice singulière");
|
|
}
|
|
++(Ipiv[icol]);
|
|
|
|
if (irow != icol)
|
|
{
|
|
for (l=0; l<n; l++)
|
|
{
|
|
temp=A[irow][l];
|
|
A[irow][l]=A[icol][l];
|
|
A[icol][l]=temp;
|
|
}
|
|
for (l=0; l<m; l++)
|
|
{
|
|
temp=B[irow][l];
|
|
B[irow][l]=B[icol][l];
|
|
B[icol][l]=temp;
|
|
}
|
|
}
|
|
|
|
Indxr[i]=irow;
|
|
Indxc[i]=icol;
|
|
if (fabs(A[icol][icol])<=EPSILON_DIV_ZERO_DOUBLE)
|
|
printf("Erreur d'inversion pt 2: matrice singulière");
|
|
pivinv=1.0/A[icol][icol];
|
|
A[icol][icol]=1.0;
|
|
for (l=0; l<n; l++)
|
|
A[icol][l]*=pivinv;
|
|
for (l=0; l<m; l++)
|
|
B[icol][l]*=pivinv;
|
|
|
|
for (ll=0; ll<n; ll++)
|
|
{
|
|
if (ll != icol)
|
|
{
|
|
dum=A[ll][icol];
|
|
A[ll][icol]=0.0;
|
|
for (l=0; l<n; l++)
|
|
A[ll][l]-=A[icol][l]*dum;
|
|
for (l=0; l<m; l++)
|
|
B[ll][l]-=B[icol][l]*dum;
|
|
}
|
|
}
|
|
|
|
}
|
|
for (l=n-1; l>=0; l--)
|
|
{
|
|
if (Indxr[l] != Indxc[l])
|
|
for (k=0; k<n; k++)
|
|
{
|
|
temp=A[k][Indxr[l]];
|
|
A[k][Indxr[l]]=A[k][Indxc[l]];
|
|
A[k][Indxc[l]]=temp;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// File: doolittle_pivot.c //
|
|
// Routines: //
|
|
// Doolittle_LU_Decomposition_with_Pivoting //
|
|
// Doolittle_LU_with_Pivoting_Solve //
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// int Doolittle_LU_Decomposition_with_Pivoting(double *A, int pivot[], //
|
|
// int n) //
|
|
// //
|
|
// Description: //
|
|
// This routine uses Doolittle's method to decompose the n x n matrix A //
|
|
// into a unit lower triangular matrix L and an upper triangular matrix U //
|
|
// such that A = LU. //
|
|
// The matrices L and U replace the matrix A so that the original matrix //
|
|
// A is destroyed. Note! In Doolittle's method the diagonal elements of //
|
|
// L are 1 and are not stored. //
|
|
// The LU decomposition is convenient when one needs to solve the linear //
|
|
// equation Ax = B for the vector x while the matrix A is fixed and the //
|
|
// vector B is varied. The routine for solving the linear system Ax = B //
|
|
// after performing the LU decomposition for A is //
|
|
// Doolittle_LU_with_Pivoting_Solve. //
|
|
// //
|
|
// The Doolittle method with partial pivoting is: Determine the pivot //
|
|
// row and interchange the current row with the pivot row, then assuming //
|
|
// that row k is the current row, k = 0, ..., n - 1 evaluate in order the //
|
|
// following pair of expressions //
|
|
// U[k][j] = A[k][j] - (L[k][0]*U[0][j] + ... + L[k][k-1]*U[k-1][j]) //
|
|
// for j = k, k+1, ... , n-1 //
|
|
// L[i][k] = (A[i][k] - (L[i][0]*U[0][k] + . + L[i][k-1]*U[k-1][k])) //
|
|
// / U[k][k] //
|
|
// for i = k+1, ... , n-1. //
|
|
// The matrix U forms the upper triangular matrix, and the matrix L //
|
|
// forms the lower triangular matrix. //
|
|
// //
|
|
// Arguments: //
|
|
// double *A Pointer to the first element of the matrix A[n][n]. //
|
|
// int pivot[] The i-th element is the pivot row interchanged with //
|
|
// row i. //
|
|
// int n The number of rows or columns of the matrix A. //
|
|
// //
|
|
// Return Values: //
|
|
// 0 Success //
|
|
// -1 Failure - The matrix A is singular. //
|
|
// //
|
|
// Example: //
|
|
// #define N //
|
|
// double A[N][N]; //
|
|
// int pivot[N]; //
|
|
// //
|
|
// (your code to create matrix A) //
|
|
// err = Doolittle_LU_Decomposition_with_Pivoting(&A[0][0], pivot, N); //
|
|
// if (err < 0) printf(" Matrix A is singular\n"); //
|
|
// else { printf(" The LU decomposition of A is \n"); //
|
|
// ... //
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// //
|
|
|
|
//#include <math.h> // required for fabs()
|
|
|
|
int Doolittle_LU_Decomposition_with_Pivoting(double *A, int pivot[], int n)
|
|
{
|
|
int i, j, k;
|
|
double *p_k = NULL, *p_row = NULL, *p_col = NULL;
|
|
double max;
|
|
|
|
|
|
// For each row and column, k = 0, ..., n-1,
|
|
|
|
for (k = 0, p_k = A; k < n; p_k += n, k++) {
|
|
|
|
// find the pivot row
|
|
|
|
pivot[k] = k;
|
|
max = fabs( *(p_k + k) );
|
|
for (j = k + 1, p_row = p_k + n; j < n; j++, p_row += n) {
|
|
if ( max < fabs(*(p_row + k)) ) {
|
|
max = fabs(*(p_row + k));
|
|
pivot[k] = j;
|
|
p_col = p_row;
|
|
}
|
|
}
|
|
|
|
// and if the pivot row differs from the current row, then
|
|
// interchange the two rows.
|
|
|
|
if (pivot[k] != k)
|
|
for (j = 0; j < n; j++) {
|
|
max = *(p_k + j);
|
|
*(p_k + j) = *(p_col + j);
|
|
*(p_col + j) = max;
|
|
}
|
|
|
|
// and if the matrix is singular, return error
|
|
|
|
|
|
if ( *(p_k + k) == 0.0 ) return -1;
|
|
|
|
// otherwise find the lower triangular matrix elements for column k.
|
|
|
|
for (i = k+1, p_row = p_k + n; i < n; p_row += n, i++) {
|
|
*(p_row + k) /= *(p_k + k);
|
|
}
|
|
|
|
// update remaining matrix
|
|
|
|
for (i = k+1, p_row = p_k + n; i < n; p_row += n, i++)
|
|
for (j = k+1; j < n; j++)
|
|
*(p_row + j) -= *(p_row + k) * *(p_k + j);
|
|
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// int Doolittle_LU_with_Pivoting_Solve(double *LU, double *B, int pivot[], //
|
|
// double *x, int n) //
|
|
// //
|
|
// Description: //
|
|
// This routine uses Doolittle's method to solve the linear equation //
|
|
// Ax = B. This routine is called after the matrix A has been decomposed //
|
|
// into a product of a unit lower triangular matrix L and an upper //
|
|
// triangular matrix U with pivoting. The argument LU is a pointer to the//
|
|
// matrix the subdiagonal part of which is L and the superdiagonal //
|
|
// together with the diagonal part is U. (The diagonal part of L is 1 and //
|
|
// is not stored.) The matrix A = LU. //
|
|
// The solution proceeds by solving the linear equation Ly = B for y and //
|
|
// subsequently solving the linear equation Ux = y for x. //
|
|
// //
|
|
// Arguments: //
|
|
// double *LU Pointer to the first element of the matrix whose //
|
|
// elements form the lower and upper triangular matrix //
|
|
// factors of A. //
|
|
// double *B Pointer to the column vector, (n x 1) matrix, B. //
|
|
// int pivot[] The i-th element is the pivot row interchanged with //
|
|
// row i. //
|
|
// double *x Solution to the equation Ax = B. //
|
|
// int n The number of rows or columns of the matrix LU. //
|
|
// //
|
|
// Return Values: //
|
|
// 0 Success //
|
|
// -1 Failure - The matrix A is singular. //
|
|
// //
|
|
// Example: //
|
|
// #define N //
|
|
// double A[N][N], B[N], x[N]; //
|
|
// int pivot[N]; //
|
|
// //
|
|
// (your code to create matrix A and column vector B) //
|
|
// err = Doolittle_LU_Decomposition_with_Pivoting(&A[0][0], pivot, N); //
|
|
// if (err < 0) printf(" Matrix A is singular\n"); //
|
|
// else { //
|
|
// err = Doolittle_LU_with_Pivoting_Solve(&A[0][0], B, pivot, x, N); //
|
|
// if (err < 0) printf(" Matrix A is singular\n"); //
|
|
// else printf(" The solution is \n"); //
|
|
// ... //
|
|
// } //
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// //
|
|
int Doolittle_LU_with_Pivoting_Solve(double *A, double B[], int pivot[],
|
|
double x[], int n)
|
|
{
|
|
int i, k;
|
|
double *p_k;
|
|
double dum;
|
|
|
|
// Solve the linear equation Lx = B for x, where L is a lower
|
|
// triangular matrix with an implied 1 along the diagonal.
|
|
|
|
for (k = 0, p_k = A; k < n; p_k += n, k++) {
|
|
if (pivot[k] != k) {dum = B[k]; B[k] = B[pivot[k]]; B[pivot[k]] = dum; }
|
|
x[k] = B[k];
|
|
for (i = 0; i < k; i++) x[k] -= x[i] * *(p_k + i);
|
|
}
|
|
|
|
// Solve the linear equation Ux = y, where y is the solution
|
|
// obtained above of Lx = B and U is an upper triangular matrix.
|
|
|
|
for (k = n-1, p_k = A + n*(n-1); k >= 0; k--, p_k -= n) {
|
|
if (pivot[k] != k) {dum = B[k]; B[k] = B[pivot[k]]; B[pivot[k]] = dum; }
|
|
for (i = k + 1; i < n; i++) x[k] -= x[i] * *(p_k + i);
|
|
if (*(p_k + k) == 0.0) return -1;
|
|
x[k] /= *(p_k + k);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Fonction calculant la norme au carré
|
|
* d'un vecteur nx1
|
|
*/
|
|
double NormeCarreVect(double* V, int n)
|
|
{
|
|
int i;
|
|
double norme=0.0f;
|
|
|
|
for (i=0; i<n; i++)
|
|
norme=norme+V[i]*V[i];
|
|
return norme;
|
|
}
|