1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
| const float DEGTORAD = 0.01745329251994329547f;
const float RADTODEG = 57.29577951308232286465f;
#define DEG2RAD(val) (val*DEGTORAD)
#define RAD2DEG(val) (val*RADTODEG)
#define RADIANS 3.141592653589
float mat[16];
void rotation_matrice(float angle_x, float angle_y, float angle_z)
{
float A = cos(angle_x);
float B = sin(angle_x);
float C = cos(angle_y);
float D = sin(angle_y);
float E = cos(angle_z);
float F = sin(angle_z);
float AD = A * D;
float BD = B * D;
mat[0] = C * E;
mat[1] = -C * F;
mat[2] = -D;
mat[4] = -BD * E + A * F;
mat[5] = BD * F + A * E;
mat[6] = -B * C;
mat[8] = AD * E + B * F;
mat[9] = -AD * F + B * E;
mat[10] = A * C;
mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
}
void matrice_angle()
{
float angle_x, angle_y, angle_z;
float C,D;
float tr_x, tr_y;
angle_y = D = -asin(mat[2]); /* Calcul de l'Angle Y */
C = cos(angle_y);
angle_y *= (float) RADIANS;
if (fabs(C) > 0.005) /* Gimbal lock ? */
{
tr_x = mat[10] / C; /* Non, donc calcul de l'angle X */
tr_y = -mat[6] / C;
angle_x = atan2(tr_y, tr_x) * (float)RADIANS;
tr_x = mat[0] / C; /* Calcul de l'angle Z */
tr_y = -mat[1] / C;
angle_z = atan2(tr_y, tr_x) * (float)RADIANS;
}
else /* Gimbal lock */
{
angle_x = 0; /* Angle X à 0 */
tr_x = mat[5]; /* Calcul de l'angle Z */
tr_y = mat[4];
angle_z = atan2(tr_y, tr_x) * (float)RADIANS;
}
char sortie[64];
sprintf(sortie, "angle_x: %f angle_y: %f angle_z: %f", RAD2DEG(angle_x),RAD2DEG(angle_y),RAD2DEG(angle_z));
MessageBox( NULL, sortie, TEXT("Import"), MB_OK|MB_ICONERROR );
//angle_x = clamp(angle_x, 0, 360);
//angle_y = clamp(angle_y, 0, 360);
//angle_z = clamp(angle_z, 0, 360);
} |
Partager