1 #include <Matematica.h>     4 Quaternione::Quaternione(){
     8 void Quaternione::AxisRotation(
double x,
double y,
double z, 
double degrees){
     9   double Angle = double((degrees / 180.0) * PI);
    11   double Result = (double)sin( Angle / 2.0 );
    13   m_w = (double)cos( Angle / 2.0 );
    15   m_x = double(x * Result);
    16   m_y = double(y * Result);
    17   m_z = double(z * Result);
    20 void Quaternione::CreateMatrix(
double *pMatrix)
    25     pMatrix[ 0] = 1.0 - 2.0 * ( m_y * m_y + m_z * m_z ); 
    26     pMatrix[ 1] = 2.0 * (m_x * m_y + m_z * m_w);
    27     pMatrix[ 2] = 2.0 * (m_x * m_z - m_y * m_w);
    30     pMatrix[ 4] = 2.0 * ( m_x * m_y - m_z * m_w );  
    31     pMatrix[ 5] = 1.0 - 2.0 * ( m_x * m_x + m_z * m_z ); 
    32     pMatrix[ 6] = 2.0 * (m_z * m_y + m_x * m_w );  
    35     pMatrix[ 8] = 2.0 * ( m_x * m_z + m_y * m_w );
    36     pMatrix[ 9] = 2.0 * ( m_y * m_z - m_x * m_w );
    37     pMatrix[10] = 1.0 - 2.0 * ( m_x * m_x + m_y * m_y );  
    50     r.m_w = m_w*q.m_w - m_x*q.m_x - m_y*q.m_y - m_z*q.m_z;
    51     r.m_x = m_w*q.m_x + m_x*q.m_w + m_y*q.m_z - m_z*q.m_y;
    52     r.m_y = m_w*q.m_y + m_y*q.m_w + m_z*q.m_x - m_x*q.m_z;
    53     r.m_z = m_w*q.m_z + m_z*q.m_w + m_x*q.m_y - m_y*q.m_x;
    61   double NormaInv = NormInv(Vett);
    62   double Sin = sin(Angle*.5);
    63   x = Sin*Vett[0]*NormaInv;
    64   y = Sin*Vett[1]*NormaInv;
    65   z = Sin*Vett[2]*NormaInv;
    69   double cp = cos(Pitch*.5);
    70   double sp = sin(Pitch*.5);
    71   double cy = cos(Yaw*.5);
    72   double sy = sin(Yaw*.5);
    73   double cr = cos(Roll*.5);
    74   double sr = sin(Roll*.5);
    75   x = sr*cp*cy - cr*sp*sy;
    76   y = cr*sp*cy + sr*cp*sy;
    77   z = cr*cp*sy - sr*sp*cy;
    78   w = cr*cp*cy + sr*sp*sy;
    91     data[NRow*0+0]  = w*w + x*x - y*y - z*z;
    92     data[NRow*0+1]  = 2.*x*y + 2.*w*z;
    93     data[NRow*0+2]  = 2.*x*z - 2.*w*y;
    96     data[NRow*1+0]  = 2.*x*y - 2.*w*z;
    97     data[NRow*1+1]  = w*w - x*x + y*y - z*z;
    98     data[NRow*1+2]  = 2.*y*z + 2.*w*x;
   101     data[NRow*2+0]  = 2.*x*z + 2.*w*y;
   102     data[NRow*2+1]  = 2.*y*z - 2.*w*x;
   103     data[NRow*2+2]  = w*w - x*x - y*y + z*z;
   109     data[NRow*3+3]  = w*w + x*x + y*y + z*z;
   113     data[NRow*0+0]  = 1. - 2.*SQR(y) - 2.*SQR(z);
   114     data[NRow*0+1]  = 2.*x*y + 2.*w*z;
   115     data[NRow*0+2]  = 2.*x*z - 2.*w*y;
   117     data[NRow*1+0]  = 2.*x*y - 2.*w*z;
   118     data[NRow*1+1]  = 1. - 2.*SQR(x) - 2.*SQR(z);
   119     data[NRow*1+2]  = 2.*y*z + 2.*w*x;
   121     data[NRow*2+0]  = 2.*x*z + 2.*w*y;
   122     data[NRow*2+1]  = 2.*y*z - 2.*w*x;
   123     data[NRow*2+2]  = 1. - 2.*SQR(x) - 2.*SQR(y);
   149   printf(
"|%lf %lf %lf %lf|\n",M[0],M[4],M[8],M[12]);
   150   printf(
"|%lf %lf %lf %lf|\n",M[1],M[5],M[9],M[13]);
   151   printf(
"|%lf %lf %lf %lf|\n",M[2],M[6],M[10],M[14]);
   152   printf(
"|%lf %lf %lf %lf|\n",M[3],M[7],M[11],M[15]);
   170   Resp = QUAD(x) + QUAD(y) + QUAD(z) + QUAD(w);
   174   double Den = 1./Norm();
   184     Norm += SQR(Vett[d]);
   185   Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
   193     Norm += SQR(Vett[d]);
   194   Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
   200   return Quadri(w * rq.
x + x * rq.
w + y * rq.
z - z * rq.
y,
   201           w * rq.
y + y * rq.
w + z * rq.
x - x * rq.
z,
   202           w * rq.
z + z * rq.
w + x * rq.
y - y * rq.
x,
   203           w * rq.
w - x * rq.
x - y * rq.
y - z * rq.
z);
   233   double Uno = w*q.
w - x*q.
x - y*q.
y - z*q.
z;
   234   double Due = w*q.
x + q.
w*x + y*q.
z - z*q.
y;
   235   double Tre = w*q.
y + q.
w*y - x*q.
z + z*q.
x;
   236   double Qua = w*q.
z + q.
w*z + x*q.
y - y*q.
x;
   244   Resp.
w = p.
w*q.
w - p.
x*q.
x - p.
y*q.
y - p.
z*q.
z;
   245   Resp.
x = p.
w*q.
x + q.
w*p.
x + p.
y*q.
z - p.
z*q.
y;
   246   Resp.
y = p.
w*q.
y + q.
w*p.
y - p.
x*q.
z + p.
z*q.
x;
   247   Resp.
z = p.
w*q.
z + q.
w*p.
z + p.
x*q.
y - p.
y*q.
x;
   251   return w*w + x*x + y*y * z*z; 
   255   double Num = 1./Sqr();
   263   M[0] = 1. - 2.*x*x - 2.*z*z;
   264   M[1] = 2.*x*y + 2.*w*z;
   265   M[2] = 2.*x*z - 2.*w*y;
   267   M[3] = 2.*x*y - 2.*w*z;
   268   M[4] = 1.-2.*x*x-2.*z*z;
   269   M[5] = 2.*x*y + 2.*w*x;
   271   M[6] = 2.*x*z + 2.*w*y;
   272   M[7] = 2.*y*z - 2.*w*x;
   273   M[8] = 1.-2.*x*x-2.*y*y;
   277   M[0] = 1. - 2.*x*x - 2.*z*z;
   278   M[1] = 2.*x*y + 2.*w*z;
   279   M[2] = 2.*x*z - 2.*w*y;
   282   M[4] = 2.*x*y - 2.*w*z;
   283   M[5] = 1.-2.*x*x-2.*z*z;
   284   M[6] = 2.*x*y + 2.*w*x;
   287   M[8] = 2.*x*z + 2.*w*y;
   288   M[9] = 2.*y*z - 2.*w*x;
   289   M[10] = 1.-2.*x*x-2.*y*y;
 Quadri operator=(const Quadri &rq) const 
Equal operator. 
void Matrix4x4(double *M)
Create a 4x4 rotation matrix. 
double Angle()
Return the rotation angle. 
double Norm()
Norm of the quaternion. 
double Sqr()
Square of a quaternion. 
double * Axis()
Print the component of the axis. 
void RotMatrix(double *data, int dim)
Alternative creation of a rotation matrix. 
double x
First basis component. 
Quadri()
An empty quaternion. 
void PrintMatrix(double *M)
Print a rotation matrix. 
double y
Second basis component. 
void Matrix3x3(double *M)
Create a 3x3 rotation matrix. 
double Normalize()
Normalize a quaternion. 
double NormInv(double *Vett)
Inverse norm of a 3d-vector. 
double w
Forth basis component. 
double z
Third basis component. 
void Basis(double a, double b, double c, double d, double *Matr)
Boh. 
Quadri operator*(const Quadri &rq) const 
Scalar product with a quaternion. 
Quadri GetConj()
Give the conjugate. 
void Prod(Quadri q)
Product between two quaternions.