mx/smargopolo
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
smargopolo/NewGon_class/MatriceVectLib.cpp
ROS Station 3023059b20 added MatriceVectLib.h & .cpp
2019-08-05 10:09:02 +02:00

1663 lines
36 KiB
C++
Raw Permalink Blame History

//********************************************
// 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;
}
Reference in New Issue View Git Blame Copy Permalink