File content as of revision 1:f0f34b17c4f0:

#include "Matrix.h"
#include "MatrixMath.h"

/*--------------------------------------------------------*/
/*--------------------------------------------------------*/
/*----------------Definition des matrices-----------------*/
/*--------------------------------------------------------*/
/*--------------------------------------------------------*/


//Valeur des rotations
Matrix Bassin(4,1);

Matrix EpauleGauche(4,1);
Matrix CoudeGauche(4,1);

Matrix EpauleDroite(4,1);
Matrix CoudeDroit(4,1);

Matrix CuisseGauche(4,1);
Matrix MolletGauche(4,1);

Matrix CuisseDroite(4,1);
Matrix MolletDroit(4,1);


//Valeurs des points des articulations
Matrix Barycentre(4,1); //valeurs à 0 par défaut

Matrix PosMilieuHautCorps(4,1);
Matrix PosMilieuBasCorps(4,1);

Matrix PosEpauleGauche(4,1);
Matrix PosEpauleDroite(4,1);

Matrix PosCoudeGauche(4,1);
Matrix PosCoudeDroit(4,1);

Matrix PosMainGauche(4,1);
Matrix PosMainDroite(4,1);

Matrix PosCuisseGauche(4,1);
Matrix PosCuisseDroite(4,1);

Matrix PosGenouGauche(4,1);
Matrix PosGenouDroit(4,1);

Matrix PosPiedGauche(4,1);
Matrix PosPiedDroit(4,1);


//Valeurs des transformations à effectuer
Matrix Trans1_EpauleGauche(4,4);
Matrix Trans2_EpauleGauche(4,4);

Matrix Trans_CoudeGauche(4,4);

Matrix Trans1_EpauleDroite(4,4);
Matrix Trans2_EpauleDroite(4,4);

Matrix Trans_CoudeDroit(4,4);

Matrix Trans1_CuisseGauche(4,4);
Matrix Trans2_CuisseGauche(4,4);

Matrix Trans_GenouGauche(4,4);

Matrix Trans1_CuisseDroite(4,4);
Matrix Trans2_CuisseDroite(4,4);

Matrix Trans_GenouDroit(4,4);


/*--------------------------------------------------------*/
/*--------------------------------------------------------*/
/*----------------Definition des fonctions----------------*/
/*--------------------------------------------------------*/
/*--------------------------------------------------------*/


void INIT_MATRICE(int mesure[][3]){
    
    //Definition des rotations au matrice des articulations  
    Bassin << mesure[0][0]
           << mesure[0][1] 
           << mesure[0][2] 
           << 1;    
                       
    EpauleGauche << mesure[1][0]
                 << mesure[1][1] 
                 << mesure[1][2] 
                 << 1; 
               
    CoudeGauche << mesure[2][0]
               << mesure[2][1] 
               << mesure[2][2] 
               << 1;           
                 
    EpauleDroite << mesure[3][0]
                 << mesure[3][1] 
                 << mesure[3][2] 
                 << 1; 
                 
    CoudeDroit << mesure[4][0]
              << mesure[4][1] 
              << mesure[4][2] 
              << 1;
              
    CuisseGauche << mesure[5][0]
                 << mesure[5][1] 
                 << mesure[5][2] 
                 << 1;
                 
    MolletGauche << mesure[6][0]
                 << mesure[6][1] 
                 << mesure[6][2] 
                 << 1; 
                
    CuisseDroite << mesure[7][0]
                 << mesure[7][1] 
                 << mesure[7][2] 
                 << 1;  
                                                                              
    MolletDroit << mesure[8][0]
                << mesure[8][1] 
                << mesure[8][2] 
                << 1; 
                
    //Definition des valeurs des points des articulations    
    Barycentre << 0 //à redéfinir
               << 0
               << 0
               << 1;
                
    PosMilieuHautCorps << Barycentre(1,1)
                       << Barycentre(2,1) + 166
                       << Barycentre(3,1)
                       << 1;
    
    PosMilieuBasCorps << Barycentre(1,1)
                      << Barycentre(2,1) - 110
                      << Barycentre(3,1)
                      << 1;
                   
    PosEpauleGauche << Barycentre(1,1) - 84
                    << Barycentre(2,1) + 166
                    << Barycentre(3,1)
                    << 1;
    
    PosEpauleDroite << Barycentre(1,1) + 84
                    << Barycentre(2,1) + 166
                    << Barycentre(3,1)
                    << 1;       
                 
    PosCuisseGauche << Barycentre(1,1) - 61.5
                    << Barycentre(2,1) - 110
                    << Barycentre(3,1)
                    << 1;
    
    PosCuisseDroite << Barycentre(1,1) + 61.5
                    << Barycentre(2,1) - 110
                    << Barycentre(3,1)
                    << 1;          
                 
    //Définition des valeurs des transformations
    
    Trans1_EpauleGauche << 1 << 0 << 0 << -84
                        << 0 << 1 << 0 << 166
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans2_EpauleGauche << 1 << 0 << 0 << 0
                        << 0 << 1 << 0 << -118
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans_CoudeGauche << 1 << 0 << 0 << 0
                      << 0 << 1 << 0 << -117
                      << 0 << 0 << 1 << 0
                      << 0 << 0 << 0 << 1;
    
    Trans1_EpauleDroite << 1 << 0 << 0 << 84
                        << 0 << 1 << 0 << 166
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans2_EpauleDroite << 1 << 0 << 0 << 0
                        << 0 << 1 << 0 << -118
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans_CoudeDroit << 1 << 0 << 0 << 0
                     << 0 << 1 << 0 << -117
                     << 0 << 0 << 1 << 0
                     << 0 << 0 << 0 << 1;
    
    Trans1_CuisseGauche << 1 << 0 << 0 << -61.5
                        << 0 << 1 << 0 << -110
                        << 0 << 0 << 1 << 0 
                        << 0 << 0 << 0 << 1;
    
    Trans2_CuisseGauche << 1 << 0 << 0 << 0 
                        << 0 << 1 << 0 << -150
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans_GenouGauche << 1 << 0 << 0 << 0
                      << 0 << 1 << 0 << -210
                      << 0 << 0 << 1 << 0
                      << 0 << 0 << 0 << 1;
    
    Trans1_CuisseDroite << 1 << 0 << 0 << 61.5
                        << 0 << 1 << 0 << -110
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans2_CuisseDroite << 1 << 0 << 0 << 0
                        << 0 << 1 << 0 << -150
                        << 0 << 0 << 1 << 0
                        << 0 << 0 << 0 << 1;
    
    Trans_GenouDroit << 1 << 0 << 0 << 0
                     << 0 << 1 << 0 << -210
                     << 0 << 0 << 1 << 0
                     << 0 << 0 << 0 << 1;                                                     
}

void AngleCalculate(float angle[]){
    PosMilieuHautCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuHautCorps;
    PosMilieuBasCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuBasCorps;
    
    PosEpauleGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleGauche;
    PosEpauleDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleDroite;
    
    Matrix RotXYZG = MatrixMath::RotX((EpauleGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((EpauleGauche.getNumber(2,1)*PI)/180.0)*MatrixMath::RotZ((EpauleGauche.getNumber(3,1)*PI)/180.0);
    PosCoudeGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*Barycentre)));
    Matrix RotXYZD = MatrixMath::RotX((EpauleDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((EpauleDroite.getNumber(2,1)*PI)/180.0)*MatrixMath::RotZ((EpauleDroite.getNumber(3,1)*PI)/180.0);
    PosCoudeDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleDroite*(RotXYZG*(Trans2_EpauleDroite*Barycentre))); 
    
    PosMainGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*(MatrixMath::RotX((EpauleGauche.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((CoudeGauche.getNumber(1,1)*PI)/180.0)*(Trans_CoudeGauche*Barycentre))))));
    PosMainDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleDroite*(MatrixMath::RotX((EpauleDroite.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((CoudeDroit.getNumber(1,1)*PI)/180.0)*(Trans_CoudeDroit*Barycentre))))));
    
    PosCuisseGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseGauche;
    PosCuisseDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseDroite;
    
    Matrix RotXZG = MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseGauche.getNumber(2,1)*PI)/180.0);
    PosGenouGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*Barycentre))); 
    Matrix RotXZD = MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseDroite.getNumber(2,1)*PI)/180.0);
    PosGenouDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*Barycentre))); 
    
    PosPiedGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*(MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((MolletGauche.getNumber(1,1)*PI)/180.0)*(Trans_GenouGauche*Barycentre))))));
    PosPiedDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*(MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((MolletDroit.getNumber(1,1)*PI)/180.0)*(Trans_GenouDroit*Barycentre))))));



}