MatematicaQuaternion.cpp
```00001 #include <Matematica.h>
00002
00003
00004 Quaternione::Quaternione(){
00005   m_x = m_y = m_z = 0.;
00006   m_w = 1.0;
00007 }
00008 void Quaternione::AxisRotation(double x,double y,double z, double degrees){
00009   double Angle = double((degrees / 180.0) * PI);
00010     // Here we calculate the sin( theta / 2) once for optimization
00011   double Result = (double)sin( Angle / 2.0 );
00012   // Calcualte the w value by cos( theta / 2 )
00013   m_w = (double)cos( Angle / 2.0 );
00014   // Calculate the x, y and z of the quaternion
00015   m_x = double(x * Result);
00016   m_y = double(y * Result);
00017   m_z = double(z * Result);
00018   //printf("%lf %lf %lf %lf\n",m_x,m_y,m_z,m_w);
00019 }
00020 void Quaternione::CreateMatrix(double *pMatrix)
00021 {
00022     // Make sure the matrix has allocated memory to store the rotation data
00023     if(!pMatrix) return;
00024     // First row
00025     pMatrix[ 0] = 1.0 - 2.0 * ( m_y * m_y + m_z * m_z );
00026     pMatrix[ 1] = 2.0 * (m_x * m_y + m_z * m_w);
00027     pMatrix[ 2] = 2.0 * (m_x * m_z - m_y * m_w);
00028     pMatrix[ 3] = 0.0;
00029     // Second row
00030     pMatrix[ 4] = 2.0 * ( m_x * m_y - m_z * m_w );
00031     pMatrix[ 5] = 1.0 - 2.0 * ( m_x * m_x + m_z * m_z );
00032     pMatrix[ 6] = 2.0 * (m_z * m_y + m_x * m_w );
00033     pMatrix[ 7] = 0.0;
00034     // Third row
00035     pMatrix[ 8] = 2.0 * ( m_x * m_z + m_y * m_w );
00036     pMatrix[ 9] = 2.0 * ( m_y * m_z - m_x * m_w );
00037     pMatrix[10] = 1.0 - 2.0 * ( m_x * m_x + m_y * m_y );
00038     pMatrix[11] = 0.0;
00039     // Fourth row
00040     pMatrix[12] = 0;
00041     pMatrix[13] = 0;
00042     pMatrix[14] = 0;
00043     pMatrix[15] = 1.0f;
00044     // Now pMatrix[] is a 4x4 homogeneous matrix that can be applied to an OpenGL Matrix
00045 }
00046
00047 Quaternione Quaternione::operator *(Quaternione q)
00048 {
00049     Quaternione r;
00050     r.m_w = m_w*q.m_w - m_x*q.m_x - m_y*q.m_y - m_z*q.m_z;
00051     r.m_x = m_w*q.m_x + m_x*q.m_w + m_y*q.m_z - m_z*q.m_y;
00052     r.m_y = m_w*q.m_y + m_y*q.m_w + m_z*q.m_x - m_x*q.m_z;
00053     r.m_z = m_w*q.m_z + m_z*q.m_w + m_x*q.m_y - m_y*q.m_x;
00054
00055     return(r);
00056 }
00058 }
00060   w = cos(Angle*.5);
00061   double NormaInv = NormInv(Vett);
00062   double Sin = sin(Angle*.5);
00063   x = Sin*Vett[0]*NormaInv;
00064   y = Sin*Vett[1]*NormaInv;
00065   z = Sin*Vett[2]*NormaInv;
00066   //printf("%lf %lf %lf %lf\n",x,y,z,w);
00067 }
00069   double cp = cos(Pitch*.5);
00070   double sp = sin(Pitch*.5);
00071   double cy = cos(Yaw*.5);
00072   double sy = sin(Yaw*.5);
00073   double cr = cos(Roll*.5);
00074   double sr = sin(Roll*.5);
00075   x = sr*cp*cy - cr*sp*sy;
00076   y = cr*sp*cy + sr*cp*sy;
00077   z = cr*cp*sy - sr*sp*cy;
00078   w = cr*cp*cy + sr*sp*sy;
00079   Normalize();
00080 }
00082   x = xx;
00083   y = yy;
00084   z = zz;
00085   w = ww;
00086 }
00088   //FIXME: the determinant is not zero!
00089   if(dim == 4){
00090     int NRow = 4;
00091     data[NRow*0+0]  = w*w + x*x - y*y - z*z;
00092     data[NRow*0+1]  = 2.*x*y + 2.*w*z;
00093     data[NRow*0+2]  = 2.*x*z - 2.*w*y;
00094     data[NRow*0+3]  = 0.;
00095
00096     data[NRow*1+0]  = 2.*x*y - 2.*w*z;
00097     data[NRow*1+1]  = w*w - x*x + y*y - z*z;
00098     data[NRow*1+2]  = 2.*y*z + 2.*w*x;
00099     data[NRow*1+3]  = 0.;
00100
00101     data[NRow*2+0]  = 2.*x*z + 2.*w*y;
00102     data[NRow*2+1]  = 2.*y*z - 2.*w*x;
00103     data[NRow*2+2]  = w*w - x*x - y*y + z*z;
00104     data[NRow*2+3]  = 0.;
00105
00106     data[NRow*3+0]  = 0.;
00107     data[NRow*3+1]  = 0.;
00108     data[NRow*3+2]  = 0.;
00109     data[NRow*3+3]  = w*w + x*x + y*y + z*z;
00110   }
00111   else{
00112     int NRow = 3;
00113     data[NRow*0+0]  = 1. - 2.*SQR(y) - 2.*SQR(z);
00114     data[NRow*0+1]  = 2.*x*y + 2.*w*z;
00115     data[NRow*0+2]  = 2.*x*z - 2.*w*y;
00116
00117     data[NRow*1+0]  = 2.*x*y - 2.*w*z;
00118     data[NRow*1+1]  = 1. - 2.*SQR(x) - 2.*SQR(z);
00119     data[NRow*1+2]  = 2.*y*z + 2.*w*x;
00120
00121     data[NRow*2+0]  = 2.*x*z + 2.*w*y;
00122     data[NRow*2+1]  = 2.*y*z - 2.*w*x;
00123     data[NRow*2+2]  = 1. - 2.*SQR(x) - 2.*SQR(y);
00124   }
00125 }
00126 void Quadri::Basis(double a,double b,double c,double d,double *M){
00127   // first row
00128   M[0] = a;
00129   M[1] = -b;
00130   M[2] = d;
00131   M[3] = c;
00132   //
00133   M[4] = b;
00134   M[5] = a;
00135   M[6] = c;
00136   M[7] = -d;
00137   //
00138   M[8] = -d;
00139   M[9] = -c;
00140   M[10]= a;
00141   M[11]= -b;
00142   //
00143   M[12] = -c;
00144   M[13] = d;
00145   M[14] = b;
00146   M[15] = a;
00147 }
00149   printf("|%lf %lf %lf %lf|\n",M[0],M[4],M[8],M[12]);
00150   printf("|%lf %lf %lf %lf|\n",M[1],M[5],M[9],M[13]);
00151   printf("|%lf %lf %lf %lf|\n",M[2],M[6],M[10],M[14]);
00152   printf("|%lf %lf %lf %lf|\n",M[3],M[7],M[11],M[15]);
00153 }
00155   x = -x;
00156   y = -y;
00157   z = -z;
00158   w = w;
00159 }
00162   Resp.x = -x;
00163   Resp.y = -y;
00164   Resp.z = -z;
00165   Resp.w = w;
00166   return Resp;
00167 }
00169   double Resp = 0.;
00171   return sqrt(Resp);
00172 }
00174   double Den = 1./Norm();
00175   x = x*Den;
00176   y = y*Den;
00177   z = z*Den;
00178   w = w*Den;
00179   return Den;
00180 }
00182   double Norm = 0.;
00183   for(int d=0;d<3;d++)
00184     Norm += SQR(Vett[d]);
00185   Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
00186   for(int d=0;d<3;d++)
00187     Vett[d] *= Norm;
00188   return 1./Norm;
00189 }
00191   double Norm = 0.;
00192   for(int d=0;d<3;d++)
00193     Norm += SQR(Vett[d]);
00194   Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
00195   return Norm;
00196 }
00198 {
00199   // the constructor takes its arguments as (x, y, z, w)
00200   return Quadri(w * rq.x + x * rq.w + y * rq.z - z * rq.y,
00201           w * rq.y + y * rq.w + z * rq.x - x * rq.z,
00202           w * rq.z + z * rq.w + x * rq.y - y * rq.x,
00203           w * rq.w - x * rq.x - y * rq.y - z * rq.z);
00204 }
00206 {
00207   // the constructor takes its arguments as (x, y, z, w)
00209 }
00210 // Multiplying a quaternion q with a vector v applies the q-rotation to v
00211 double *Quadri::operator* (const double *Vet) const
00212 {
00213   // product v' = q v q*
00214   double Resp[3];
00215   // for(int d=0;d<3;d++)
00216   //   Resp[d] = Vet[d];
00217   // Normalize(Resp);
00219   // vecQuat.x = Resp[0];
00220   // vecQuat.y = Resp[1];
00221   // vecQuat.z = Resp[2];
00222   // vecQuat.w = 0.0;
00223
00224   // resQuat = vecQuat * GetConj();
00225   // resQuat = *this * resQuat;
00226   // Resp[0] = resQuat.x;
00227   // Resp[1] = resQuat.y;
00228   // Resp[2] = resQuat.z;
00229
00230   return Resp;
00231 }
00233   double Uno = w*q.w - x*q.x - y*q.y - z*q.z;
00234   double Due = w*q.x + q.w*x + y*q.z - z*q.y;
00235   double Tre = w*q.y + q.w*y - x*q.z + z*q.x;
00236   double Qua = w*q.z + q.w*z + x*q.y - y*q.x;
00237   w = Uno;
00238   x = Due;
00239   y = Tre;
00240   z = Qua;
00241 }
00244   Resp.w = p.w*q.w - p.x*q.x - p.y*q.y - p.z*q.z;
00245   Resp.x = p.w*q.x + q.w*p.x + p.y*q.z - p.z*q.y;
00246   Resp.y = p.w*q.y + q.w*p.y - p.x*q.z + p.z*q.x;
00247   Resp.z = p.w*q.z + q.w*p.z + p.x*q.y - p.y*q.x;
00249 }
00251   return w*w + x*x + y*y * z*z;
00252 }
00254   Conj();
00255   double Num = 1./Sqr();
00256   w = w*Num;
00257   x = x*Num;
00258   y = y*Num;
00259   z = z*Num;
00260 }
00262   // first row
00263   M[0] = 1. - 2.*x*x - 2.*z*z;
00264   M[1] = 2.*x*y + 2.*w*z;
00265   M[2] = 2.*x*z - 2.*w*y;
00266   //
00267   M[3] = 2.*x*y - 2.*w*z;
00268   M[4] = 1.-2.*x*x-2.*z*z;
00269   M[5] = 2.*x*y + 2.*w*x;
00270   //
00271   M[6] = 2.*x*z + 2.*w*y;
00272   M[7] = 2.*y*z - 2.*w*x;
00273   M[8] = 1.-2.*x*x-2.*y*y;
00274 }
00276   // first row
00277   M[0] = 1. - 2.*x*x - 2.*z*z;
00278   M[1] = 2.*x*y + 2.*w*z;
00279   M[2] = 2.*x*z - 2.*w*y;
00280   M[3] = 0.;
00281   //
00282   M[4] = 2.*x*y - 2.*w*z;
00283   M[5] = 1.-2.*x*x-2.*z*z;
00284   M[6] = 2.*x*y + 2.*w*x;
00285   M[7] = 0.;
00286   //
00287   M[8] = 2.*x*z + 2.*w*y;
00288   M[9] = 2.*y*z - 2.*w*x;
00289   M[10] = 1.-2.*x*x-2.*y*y;
00290   M[11] = 0.;
00291   //
00292   M[12] = 0.;
00293   M[13] = 0.;
00294   M[14] = 0.;
00295   M[15] = 1.;
00296 }
00298   double v[3];
00299   double Den = Norm();
00300   v[0] = x/Den;
00301   v[1] = y/Den;
00302   v[2] = z/Den;
00303   return v;
00304 }