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 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103
| float angleFromCosAndSin(float cosinus, float sinus)
{
float sol1 = acos(cosinus);
float sol2 = -sol1;
if(sin(sol1) == sinus)
return sol1;
if(sin(sol2) == sinus)
return sol2;
char c[128];
sprintf(c, "pas de solution : cos=%f, sin=%f, sol1=%f, sol2=%f\n", cosinus, sinus, sol1, sol2);
printf("pas de solution : cos=%f, sin=%f, sol1=%f, sol2=%f\n", cosinus, sinus, sol1, sol2);
return -1;
}
void crossProd(float v1[3], float v2[3], float out[3]) // Calculates Normal For A Quad Using 3 Points
{
static const int x = 0; // Define X Coord
static const int y = 1; // Define Y Coord
static const int z = 2; // Define Z Coord
// Compute The Cross Product To Give Us A Surface Normal
out[x] = v1[y]*v2[z] - v1[z]*v2[y]; // Cross Product For Y - Z
out[y] = v1[z]*v2[x] - v1[x]*v2[z]; // Cross Product For X - Z
out[z] = v1[x]*v2[y] - v1[y]*v2[x]; // Cross Product For X - Y
}
void scalarProd(float v1[3], float v2[3], float &out) // Calculates Normal For A Quad Using 3 Points
{
static const int x = 0; // Define X Coord
static const int y = 1; // Define Y Coord
static const int z = 2; // Define Z Coord
// Compute The Scalar Product
out = v1[x]*v2[x] + v1[y]*v2[y] + v1[z]*v2[z];
}
void quatProd(float u1, float v1[3], float u2, float v2[3], float &uOut, float vOut[3])
{
static const int x = 0; // Define X Coord
static const int y = 1; // Define Y Coord
static const int z = 2; // Define Z Coord
float scalProd;
float vectProd[3];
scalarProd(v1, v2, scalProd);
uOut = u1*u2 - scalProd;
crossProd(v1, v2, vectProd);
vOut[0] = u1*v2[0] + u2*v1[0] + vectProd[0];
vOut[1] = u1*v2[1] + u2*v1[1] + vectProd[1];
vOut[2] = u1*v2[2] + u2*v1[2] + vectProd[2];
}
float norme(float vec[3])
{
return sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);
}
void normaliser(float vec[3])
{
float norm=norme(vec);
int i;
for(i=0;i<3;i++)
vec[i]=vec[i]/norm;
}
void rotationToQuaternion(float angle, float vec[3], float &uOut, float vOut[3])
{
int i;
static const int x = 0; // Define X Coord
static const int y = 1; // Define Y Coord
static const int z = 2; // Define Z Coord
normaliser(vOut);
uOut = cos(angle/2);
for(i=0;i<3;i++)
vOut[0] = vec[0]*sin(angle/2);
}
void quaternionToRotation(float u, float v[3], float &angle, float vec[3])
{
int i;
float cos = u;
float sin = norme(v);
angle = 2 * angleFromCosAndSin(cos, sin);
for(i=0;i<3;i++)
vec[i] = v[i]/sin;
}
void composee(float angle1, float vec1[3], float angle2, float vec2[3], float &angleOut, float vecOut[3])
{
float u1, u2, uRes;
float v1[3], v2[3], vRes[3];
rotationToQuaternion(angle1, vec1, u1, v1);
rotationToQuaternion(angle2, vec2, u2, v2);
quatProd(u2, v2, u1, v1, uRes, vRes);
quaternionToRotation(uRes, vRes, angleOut, vecOut);
}
ds le main :
float angle1, angle2, angleOut;
float vec1[3], vec2[3], vecOut[3];
angle1=90.0f; vec1[0]=1.0f; vec1[1]=0.0f; vec1[2]=0.0f;
angle2=90.0f; vec2[0]=0.0f; vec2[1]=1.0f; vec2[2]=0.0f;
composee(angle1, vec1, angle2, vec2, angleOut, vecOut); |
Partager