Screen-Puppet
Dependencies: Matrix MatrixMath PCA9547 PowerControl mbed
Fork of mbed_multiplex by
CalculMatrix.h@1:f0f34b17c4f0, 2015-09-04 (annotated)
- Committer:
- yenzo
- Date:
- Fri Sep 04 21:44:01 2015 +0000
- Revision:
- 1:f0f34b17c4f0
With matrix
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yenzo | 1:f0f34b17c4f0 | 1 | #include "Matrix.h" |
yenzo | 1:f0f34b17c4f0 | 2 | #include "MatrixMath.h" |
yenzo | 1:f0f34b17c4f0 | 3 | |
yenzo | 1:f0f34b17c4f0 | 4 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 5 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 6 | /*----------------Definition des matrices-----------------*/ |
yenzo | 1:f0f34b17c4f0 | 7 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 8 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 9 | |
yenzo | 1:f0f34b17c4f0 | 10 | |
yenzo | 1:f0f34b17c4f0 | 11 | //Valeur des rotations |
yenzo | 1:f0f34b17c4f0 | 12 | Matrix Bassin(4,1); |
yenzo | 1:f0f34b17c4f0 | 13 | |
yenzo | 1:f0f34b17c4f0 | 14 | Matrix EpauleGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 15 | Matrix CoudeGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 16 | |
yenzo | 1:f0f34b17c4f0 | 17 | Matrix EpauleDroite(4,1); |
yenzo | 1:f0f34b17c4f0 | 18 | Matrix CoudeDroit(4,1); |
yenzo | 1:f0f34b17c4f0 | 19 | |
yenzo | 1:f0f34b17c4f0 | 20 | Matrix CuisseGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 21 | Matrix MolletGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 22 | |
yenzo | 1:f0f34b17c4f0 | 23 | Matrix CuisseDroite(4,1); |
yenzo | 1:f0f34b17c4f0 | 24 | Matrix MolletDroit(4,1); |
yenzo | 1:f0f34b17c4f0 | 25 | |
yenzo | 1:f0f34b17c4f0 | 26 | |
yenzo | 1:f0f34b17c4f0 | 27 | //Valeurs des points des articulations |
yenzo | 1:f0f34b17c4f0 | 28 | Matrix Barycentre(4,1); //valeurs à 0 par défaut |
yenzo | 1:f0f34b17c4f0 | 29 | |
yenzo | 1:f0f34b17c4f0 | 30 | Matrix PosMilieuHautCorps(4,1); |
yenzo | 1:f0f34b17c4f0 | 31 | Matrix PosMilieuBasCorps(4,1); |
yenzo | 1:f0f34b17c4f0 | 32 | |
yenzo | 1:f0f34b17c4f0 | 33 | Matrix PosEpauleGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 34 | Matrix PosEpauleDroite(4,1); |
yenzo | 1:f0f34b17c4f0 | 35 | |
yenzo | 1:f0f34b17c4f0 | 36 | Matrix PosCoudeGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 37 | Matrix PosCoudeDroit(4,1); |
yenzo | 1:f0f34b17c4f0 | 38 | |
yenzo | 1:f0f34b17c4f0 | 39 | Matrix PosMainGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 40 | Matrix PosMainDroite(4,1); |
yenzo | 1:f0f34b17c4f0 | 41 | |
yenzo | 1:f0f34b17c4f0 | 42 | Matrix PosCuisseGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 43 | Matrix PosCuisseDroite(4,1); |
yenzo | 1:f0f34b17c4f0 | 44 | |
yenzo | 1:f0f34b17c4f0 | 45 | Matrix PosGenouGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 46 | Matrix PosGenouDroit(4,1); |
yenzo | 1:f0f34b17c4f0 | 47 | |
yenzo | 1:f0f34b17c4f0 | 48 | Matrix PosPiedGauche(4,1); |
yenzo | 1:f0f34b17c4f0 | 49 | Matrix PosPiedDroit(4,1); |
yenzo | 1:f0f34b17c4f0 | 50 | |
yenzo | 1:f0f34b17c4f0 | 51 | |
yenzo | 1:f0f34b17c4f0 | 52 | //Valeurs des transformations à effectuer |
yenzo | 1:f0f34b17c4f0 | 53 | Matrix Trans1_EpauleGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 54 | Matrix Trans2_EpauleGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 55 | |
yenzo | 1:f0f34b17c4f0 | 56 | Matrix Trans_CoudeGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 57 | |
yenzo | 1:f0f34b17c4f0 | 58 | Matrix Trans1_EpauleDroite(4,4); |
yenzo | 1:f0f34b17c4f0 | 59 | Matrix Trans2_EpauleDroite(4,4); |
yenzo | 1:f0f34b17c4f0 | 60 | |
yenzo | 1:f0f34b17c4f0 | 61 | Matrix Trans_CoudeDroit(4,4); |
yenzo | 1:f0f34b17c4f0 | 62 | |
yenzo | 1:f0f34b17c4f0 | 63 | Matrix Trans1_CuisseGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 64 | Matrix Trans2_CuisseGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 65 | |
yenzo | 1:f0f34b17c4f0 | 66 | Matrix Trans_GenouGauche(4,4); |
yenzo | 1:f0f34b17c4f0 | 67 | |
yenzo | 1:f0f34b17c4f0 | 68 | Matrix Trans1_CuisseDroite(4,4); |
yenzo | 1:f0f34b17c4f0 | 69 | Matrix Trans2_CuisseDroite(4,4); |
yenzo | 1:f0f34b17c4f0 | 70 | |
yenzo | 1:f0f34b17c4f0 | 71 | Matrix Trans_GenouDroit(4,4); |
yenzo | 1:f0f34b17c4f0 | 72 | |
yenzo | 1:f0f34b17c4f0 | 73 | |
yenzo | 1:f0f34b17c4f0 | 74 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 75 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 76 | /*----------------Definition des fonctions----------------*/ |
yenzo | 1:f0f34b17c4f0 | 77 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 78 | /*--------------------------------------------------------*/ |
yenzo | 1:f0f34b17c4f0 | 79 | |
yenzo | 1:f0f34b17c4f0 | 80 | |
yenzo | 1:f0f34b17c4f0 | 81 | void INIT_MATRICE(int mesure[][3]){ |
yenzo | 1:f0f34b17c4f0 | 82 | |
yenzo | 1:f0f34b17c4f0 | 83 | //Definition des rotations au matrice des articulations |
yenzo | 1:f0f34b17c4f0 | 84 | Bassin << mesure[0][0] |
yenzo | 1:f0f34b17c4f0 | 85 | << mesure[0][1] |
yenzo | 1:f0f34b17c4f0 | 86 | << mesure[0][2] |
yenzo | 1:f0f34b17c4f0 | 87 | << 1; |
yenzo | 1:f0f34b17c4f0 | 88 | |
yenzo | 1:f0f34b17c4f0 | 89 | EpauleGauche << mesure[1][0] |
yenzo | 1:f0f34b17c4f0 | 90 | << mesure[1][1] |
yenzo | 1:f0f34b17c4f0 | 91 | << mesure[1][2] |
yenzo | 1:f0f34b17c4f0 | 92 | << 1; |
yenzo | 1:f0f34b17c4f0 | 93 | |
yenzo | 1:f0f34b17c4f0 | 94 | CoudeGauche << mesure[2][0] |
yenzo | 1:f0f34b17c4f0 | 95 | << mesure[2][1] |
yenzo | 1:f0f34b17c4f0 | 96 | << mesure[2][2] |
yenzo | 1:f0f34b17c4f0 | 97 | << 1; |
yenzo | 1:f0f34b17c4f0 | 98 | |
yenzo | 1:f0f34b17c4f0 | 99 | EpauleDroite << mesure[3][0] |
yenzo | 1:f0f34b17c4f0 | 100 | << mesure[3][1] |
yenzo | 1:f0f34b17c4f0 | 101 | << mesure[3][2] |
yenzo | 1:f0f34b17c4f0 | 102 | << 1; |
yenzo | 1:f0f34b17c4f0 | 103 | |
yenzo | 1:f0f34b17c4f0 | 104 | CoudeDroit << mesure[4][0] |
yenzo | 1:f0f34b17c4f0 | 105 | << mesure[4][1] |
yenzo | 1:f0f34b17c4f0 | 106 | << mesure[4][2] |
yenzo | 1:f0f34b17c4f0 | 107 | << 1; |
yenzo | 1:f0f34b17c4f0 | 108 | |
yenzo | 1:f0f34b17c4f0 | 109 | CuisseGauche << mesure[5][0] |
yenzo | 1:f0f34b17c4f0 | 110 | << mesure[5][1] |
yenzo | 1:f0f34b17c4f0 | 111 | << mesure[5][2] |
yenzo | 1:f0f34b17c4f0 | 112 | << 1; |
yenzo | 1:f0f34b17c4f0 | 113 | |
yenzo | 1:f0f34b17c4f0 | 114 | MolletGauche << mesure[6][0] |
yenzo | 1:f0f34b17c4f0 | 115 | << mesure[6][1] |
yenzo | 1:f0f34b17c4f0 | 116 | << mesure[6][2] |
yenzo | 1:f0f34b17c4f0 | 117 | << 1; |
yenzo | 1:f0f34b17c4f0 | 118 | |
yenzo | 1:f0f34b17c4f0 | 119 | CuisseDroite << mesure[7][0] |
yenzo | 1:f0f34b17c4f0 | 120 | << mesure[7][1] |
yenzo | 1:f0f34b17c4f0 | 121 | << mesure[7][2] |
yenzo | 1:f0f34b17c4f0 | 122 | << 1; |
yenzo | 1:f0f34b17c4f0 | 123 | |
yenzo | 1:f0f34b17c4f0 | 124 | MolletDroit << mesure[8][0] |
yenzo | 1:f0f34b17c4f0 | 125 | << mesure[8][1] |
yenzo | 1:f0f34b17c4f0 | 126 | << mesure[8][2] |
yenzo | 1:f0f34b17c4f0 | 127 | << 1; |
yenzo | 1:f0f34b17c4f0 | 128 | |
yenzo | 1:f0f34b17c4f0 | 129 | //Definition des valeurs des points des articulations |
yenzo | 1:f0f34b17c4f0 | 130 | Barycentre << 0 //à redéfinir |
yenzo | 1:f0f34b17c4f0 | 131 | << 0 |
yenzo | 1:f0f34b17c4f0 | 132 | << 0 |
yenzo | 1:f0f34b17c4f0 | 133 | << 1; |
yenzo | 1:f0f34b17c4f0 | 134 | |
yenzo | 1:f0f34b17c4f0 | 135 | PosMilieuHautCorps << Barycentre(1,1) |
yenzo | 1:f0f34b17c4f0 | 136 | << Barycentre(2,1) + 166 |
yenzo | 1:f0f34b17c4f0 | 137 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 138 | << 1; |
yenzo | 1:f0f34b17c4f0 | 139 | |
yenzo | 1:f0f34b17c4f0 | 140 | PosMilieuBasCorps << Barycentre(1,1) |
yenzo | 1:f0f34b17c4f0 | 141 | << Barycentre(2,1) - 110 |
yenzo | 1:f0f34b17c4f0 | 142 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 143 | << 1; |
yenzo | 1:f0f34b17c4f0 | 144 | |
yenzo | 1:f0f34b17c4f0 | 145 | PosEpauleGauche << Barycentre(1,1) - 84 |
yenzo | 1:f0f34b17c4f0 | 146 | << Barycentre(2,1) + 166 |
yenzo | 1:f0f34b17c4f0 | 147 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 148 | << 1; |
yenzo | 1:f0f34b17c4f0 | 149 | |
yenzo | 1:f0f34b17c4f0 | 150 | PosEpauleDroite << Barycentre(1,1) + 84 |
yenzo | 1:f0f34b17c4f0 | 151 | << Barycentre(2,1) + 166 |
yenzo | 1:f0f34b17c4f0 | 152 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 153 | << 1; |
yenzo | 1:f0f34b17c4f0 | 154 | |
yenzo | 1:f0f34b17c4f0 | 155 | PosCuisseGauche << Barycentre(1,1) - 61.5 |
yenzo | 1:f0f34b17c4f0 | 156 | << Barycentre(2,1) - 110 |
yenzo | 1:f0f34b17c4f0 | 157 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 158 | << 1; |
yenzo | 1:f0f34b17c4f0 | 159 | |
yenzo | 1:f0f34b17c4f0 | 160 | PosCuisseDroite << Barycentre(1,1) + 61.5 |
yenzo | 1:f0f34b17c4f0 | 161 | << Barycentre(2,1) - 110 |
yenzo | 1:f0f34b17c4f0 | 162 | << Barycentre(3,1) |
yenzo | 1:f0f34b17c4f0 | 163 | << 1; |
yenzo | 1:f0f34b17c4f0 | 164 | |
yenzo | 1:f0f34b17c4f0 | 165 | //Définition des valeurs des transformations |
yenzo | 1:f0f34b17c4f0 | 166 | |
yenzo | 1:f0f34b17c4f0 | 167 | Trans1_EpauleGauche << 1 << 0 << 0 << -84 |
yenzo | 1:f0f34b17c4f0 | 168 | << 0 << 1 << 0 << 166 |
yenzo | 1:f0f34b17c4f0 | 169 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 170 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 171 | |
yenzo | 1:f0f34b17c4f0 | 172 | Trans2_EpauleGauche << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 173 | << 0 << 1 << 0 << -118 |
yenzo | 1:f0f34b17c4f0 | 174 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 175 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 176 | |
yenzo | 1:f0f34b17c4f0 | 177 | Trans_CoudeGauche << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 178 | << 0 << 1 << 0 << -117 |
yenzo | 1:f0f34b17c4f0 | 179 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 180 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 181 | |
yenzo | 1:f0f34b17c4f0 | 182 | Trans1_EpauleDroite << 1 << 0 << 0 << 84 |
yenzo | 1:f0f34b17c4f0 | 183 | << 0 << 1 << 0 << 166 |
yenzo | 1:f0f34b17c4f0 | 184 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 185 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 186 | |
yenzo | 1:f0f34b17c4f0 | 187 | Trans2_EpauleDroite << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 188 | << 0 << 1 << 0 << -118 |
yenzo | 1:f0f34b17c4f0 | 189 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 190 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 191 | |
yenzo | 1:f0f34b17c4f0 | 192 | Trans_CoudeDroit << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 193 | << 0 << 1 << 0 << -117 |
yenzo | 1:f0f34b17c4f0 | 194 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 195 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 196 | |
yenzo | 1:f0f34b17c4f0 | 197 | Trans1_CuisseGauche << 1 << 0 << 0 << -61.5 |
yenzo | 1:f0f34b17c4f0 | 198 | << 0 << 1 << 0 << -110 |
yenzo | 1:f0f34b17c4f0 | 199 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 200 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 201 | |
yenzo | 1:f0f34b17c4f0 | 202 | Trans2_CuisseGauche << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 203 | << 0 << 1 << 0 << -150 |
yenzo | 1:f0f34b17c4f0 | 204 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 205 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 206 | |
yenzo | 1:f0f34b17c4f0 | 207 | Trans_GenouGauche << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 208 | << 0 << 1 << 0 << -210 |
yenzo | 1:f0f34b17c4f0 | 209 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 210 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 211 | |
yenzo | 1:f0f34b17c4f0 | 212 | Trans1_CuisseDroite << 1 << 0 << 0 << 61.5 |
yenzo | 1:f0f34b17c4f0 | 213 | << 0 << 1 << 0 << -110 |
yenzo | 1:f0f34b17c4f0 | 214 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 215 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 216 | |
yenzo | 1:f0f34b17c4f0 | 217 | Trans2_CuisseDroite << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 218 | << 0 << 1 << 0 << -150 |
yenzo | 1:f0f34b17c4f0 | 219 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 220 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 221 | |
yenzo | 1:f0f34b17c4f0 | 222 | Trans_GenouDroit << 1 << 0 << 0 << 0 |
yenzo | 1:f0f34b17c4f0 | 223 | << 0 << 1 << 0 << -210 |
yenzo | 1:f0f34b17c4f0 | 224 | << 0 << 0 << 1 << 0 |
yenzo | 1:f0f34b17c4f0 | 225 | << 0 << 0 << 0 << 1; |
yenzo | 1:f0f34b17c4f0 | 226 | } |
yenzo | 1:f0f34b17c4f0 | 227 | |
yenzo | 1:f0f34b17c4f0 | 228 | void AngleCalculate(float angle[]){ |
yenzo | 1:f0f34b17c4f0 | 229 | PosMilieuHautCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuHautCorps; |
yenzo | 1:f0f34b17c4f0 | 230 | PosMilieuBasCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuBasCorps; |
yenzo | 1:f0f34b17c4f0 | 231 | |
yenzo | 1:f0f34b17c4f0 | 232 | PosEpauleGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleGauche; |
yenzo | 1:f0f34b17c4f0 | 233 | PosEpauleDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleDroite; |
yenzo | 1:f0f34b17c4f0 | 234 | |
yenzo | 1:f0f34b17c4f0 | 235 | 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); |
yenzo | 1:f0f34b17c4f0 | 236 | PosCoudeGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*Barycentre))); |
yenzo | 1:f0f34b17c4f0 | 237 | 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); |
yenzo | 1:f0f34b17c4f0 | 238 | PosCoudeDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleDroite*(RotXYZG*(Trans2_EpauleDroite*Barycentre))); |
yenzo | 1:f0f34b17c4f0 | 239 | |
yenzo | 1:f0f34b17c4f0 | 240 | 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)))))); |
yenzo | 1:f0f34b17c4f0 | 241 | 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)))))); |
yenzo | 1:f0f34b17c4f0 | 242 | |
yenzo | 1:f0f34b17c4f0 | 243 | PosCuisseGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseGauche; |
yenzo | 1:f0f34b17c4f0 | 244 | PosCuisseDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseDroite; |
yenzo | 1:f0f34b17c4f0 | 245 | |
yenzo | 1:f0f34b17c4f0 | 246 | Matrix RotXZG = MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseGauche.getNumber(2,1)*PI)/180.0); |
yenzo | 1:f0f34b17c4f0 | 247 | PosGenouGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*Barycentre))); |
yenzo | 1:f0f34b17c4f0 | 248 | Matrix RotXZD = MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseDroite.getNumber(2,1)*PI)/180.0); |
yenzo | 1:f0f34b17c4f0 | 249 | PosGenouDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*Barycentre))); |
yenzo | 1:f0f34b17c4f0 | 250 | |
yenzo | 1:f0f34b17c4f0 | 251 | 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)))))); |
yenzo | 1:f0f34b17c4f0 | 252 | 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)))))); |
yenzo | 1:f0f34b17c4f0 | 253 | |
yenzo | 1:f0f34b17c4f0 | 254 | |
yenzo | 1:f0f34b17c4f0 | 255 | |
yenzo | 1:f0f34b17c4f0 | 256 | } |