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
| float * getRotAngle(float a[3], float b[3])
{
// le vecteur
float v[3]={ b[0] - a[0] , b[1] - a[1] , b[2] - a[2] };
// la longueur
float lg= distance(a, b);
// calcul de rotation par rapport au vecteur OZ;
float OZ[3]= { 0, 0 , 1};
float vn[3]= { v[0]/lg , v[1]/lg , v[2]/lg };
// produit_normes = norm(OZ) * norm(vn);
float prod_norm= sqrt( pow(OZ[0],2) + pow(OZ[1],2) + pow(OZ[2],2)) * sqrt(pow(vn[0],2) + pow(vn[1],2) + pow(vn[2],2));
// produit_scalaire= dot(OZ , vn);
float prod_scalar= produitScalaire(vn, OZ) ;
// produit_vectoriel= cross(OZ , vn);
float prod_vect[3];
produitVectoriel(OZ, vn, prod_vect); // non-normalisé
//cos_phi= produit_scalaire / produit_normes ;
float cos_phi= prod_scalar / prod_norm;
//sin_phi= norm(produit_vectoriel) / produit_normes;
float sin_phi= sqrt( pow(prod_vect[0],2) + pow(prod_vect[1],2) + pow(prod_vect[2],2) ) / prod_norm;
float phi= acos(cos_phi);
if (phi > PI/2)
{
phi = PI - phi;
}
float N[3];
produitVectoriel(OZ, vn, N);
int d= normalise(N);
float ux= N[0];
float uy= N[1];
float uz= N[2];
float c= cos(phi);
float s= sin(phi);
float angle = 180 - 180 * phi / PI;
float *output= new float[4];
output[0] = angle;
output[1] = ux;
output[2] = uy;
output[3] = uz;
return output;
} |
Partager