Allink  v0.1
MatematicaQuaternion.cpp
1 #include <Matematica.h>
2 
3 
4 Quaternione::Quaternione(){
5  m_x = m_y = m_z = 0.;
6  m_w = 1.0;
7 }
8 void Quaternione::AxisRotation(double x,double y,double z, double degrees){
9  double Angle = double((degrees / 180.0) * PI);
10  // Here we calculate the sin( theta / 2) once for optimization
11  double Result = (double)sin( Angle / 2.0 );
12  // Calcualte the w value by cos( theta / 2 )
13  m_w = (double)cos( Angle / 2.0 );
14  // Calculate the x, y and z of the quaternion
15  m_x = double(x * Result);
16  m_y = double(y * Result);
17  m_z = double(z * Result);
18  //printf("%lf %lf %lf %lf\n",m_x,m_y,m_z,m_w);
19 }
20 void Quaternione::CreateMatrix(double *pMatrix)
21 {
22  // Make sure the matrix has allocated memory to store the rotation data
23  if(!pMatrix) return;
24  // First row
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);
28  pMatrix[ 3] = 0.0;
29  // Second row
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 );
33  pMatrix[ 7] = 0.0;
34  // Third row
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 );
38  pMatrix[11] = 0.0;
39  // Fourth row
40  pMatrix[12] = 0;
41  pMatrix[13] = 0;
42  pMatrix[14] = 0;
43  pMatrix[15] = 1.0f;
44  // Now pMatrix[] is a 4x4 homogeneous matrix that can be applied to an OpenGL Matrix
45 }
46 
47 Quaternione Quaternione::operator *(Quaternione q)
48 {
49  Quaternione r;
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;
54 
55  return(r);
56 }
58 }
59 Quadri::Quadri(double *Vett,double Angle){
60  w = cos(Angle*.5);
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;
66  //printf("%lf %lf %lf %lf\n",x,y,z,w);
67 }
68 Quadri::Quadri(double Pitch,double Yaw,double Roll){
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;
79  Normalize();
80 }
81 Quadri::Quadri(double ww,double xx,double yy,double zz){
82  x = xx;
83  y = yy;
84  z = zz;
85  w = ww;
86 }
87 void Quadri::RotMatrix(double *data,int dim){
88  //FIXME: the determinant is not zero!
89  if(dim == 4){
90  int NRow = 4;
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;
94  data[NRow*0+3] = 0.;
95 
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;
99  data[NRow*1+3] = 0.;
100 
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;
104  data[NRow*2+3] = 0.;
105 
106  data[NRow*3+0] = 0.;
107  data[NRow*3+1] = 0.;
108  data[NRow*3+2] = 0.;
109  data[NRow*3+3] = w*w + x*x + y*y + z*z;
110  }
111  else{
112  int NRow = 3;
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;
116 
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;
120 
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);
124  }
125 }
126 void Quadri::Basis(double a,double b,double c,double d,double *M){
127  // first row
128  M[0] = a;
129  M[1] = -b;
130  M[2] = d;
131  M[3] = c;
132  //
133  M[4] = b;
134  M[5] = a;
135  M[6] = c;
136  M[7] = -d;
137  //
138  M[8] = -d;
139  M[9] = -c;
140  M[10]= a;
141  M[11]= -b;
142  //
143  M[12] = -c;
144  M[13] = d;
145  M[14] = b;
146  M[15] = a;
147 }
148 void Quadri::PrintMatrix(double *M){
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]);
153 }
155  x = -x;
156  y = -y;
157  z = -z;
158  w = w;
159 }
161  Quadri Resp;
162  Resp.x = -x;
163  Resp.y = -y;
164  Resp.z = -z;
165  Resp.w = w;
166  return Resp;
167 }
168 double Quadri::Norm(){
169  double Resp = 0.;
170  Resp = QUAD(x) + QUAD(y) + QUAD(z) + QUAD(w);
171  return sqrt(Resp);
172 }
174  double Den = 1./Norm();
175  x = x*Den;
176  y = y*Den;
177  z = z*Den;
178  w = w*Den;
179  return Den;
180 }
181 double Quadri::Normalize(double *Vett){
182  double Norm = 0.;
183  for(int d=0;d<3;d++)
184  Norm += SQR(Vett[d]);
185  Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
186  for(int d=0;d<3;d++)
187  Vett[d] *= Norm;
188  return 1./Norm;
189 }
190 double Quadri::NormInv(double *Vett){
191  double Norm = 0.;
192  for(int d=0;d<3;d++)
193  Norm += SQR(Vett[d]);
194  Norm = Norm > 0. ? 1./sqrt(Norm) : 1.;
195  return Norm;
196 }
198 {
199  // the constructor takes its arguments as (x, y, z, w)
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);
204 }
206 {
207  // the constructor takes its arguments as (x, y, z, w)
208  return Quadri(rq.w,rq.x,rq.y,rq.z);
209 }
210 // Multiplying a quaternion q with a vector v applies the q-rotation to v
211 double *Quadri::operator* (const double *Vet) const
212 {
213  // product v' = q v q*
214  double Resp[3];
215  // for(int d=0;d<3;d++)
216  // Resp[d] = Vet[d];
217  // Normalize(Resp);
218  // Quadri vecQuat, resQuat;
219  // vecQuat.x = Resp[0];
220  // vecQuat.y = Resp[1];
221  // vecQuat.z = Resp[2];
222  // vecQuat.w = 0.0;
223 
224  // resQuat = vecQuat * GetConj();
225  // resQuat = *this * resQuat;
226  // Resp[0] = resQuat.x;
227  // Resp[1] = resQuat.y;
228  // Resp[2] = resQuat.z;
229 
230  return Resp;
231 }
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;
237  w = Uno;
238  x = Due;
239  y = Tre;
240  z = Qua;
241 }
243  Quadri Resp;
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;
248  //return Quadri;
249 }
250 double Quadri::Sqr(){
251  return w*w + x*x + y*y * z*z;
252 }
253 void Quadri::Inv(){
254  Conj();
255  double Num = 1./Sqr();
256  w = w*Num;
257  x = x*Num;
258  y = y*Num;
259  z = z*Num;
260 }
261 void Quadri::Matrix3x3(double *M){
262  // first row
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;
266  //
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;
270  //
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;
274 }
275 void Quadri::Matrix4x4(double *M){
276  // first row
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;
280  M[3] = 0.;
281  //
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;
285  M[7] = 0.;
286  //
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;
290  M[11] = 0.;
291  //
292  M[12] = 0.;
293  M[13] = 0.;
294  M[14] = 0.;
295  M[15] = 1.;
296 }
297 double *Quadri::Axis(){
298  double v[3];
299  double Den = Norm();
300  v[0] = x/Den;
301  v[1] = y/Den;
302  v[2] = z/Den;
303  return v;
304 }
305 double Quadri::Angle(){
306  return 2.*acos(w);
307 }
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.
void Inv()
Inverse.
void Conj()
Conjugate.
double y
Second basis component.
void Matrix3x3(double *M)
Create a 3x3 rotation matrix.
double Normalize()
Normalize a quaternion.
Quaternion operations.
double NormInv(double *Vett)
Inverse norm of a 3d-vector.
double w
Forth basis component.
Quaternion class.
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.