Ansicht | Historie | Commit | Commitdiff
Janis Streib
19.05.23 b21ff09151ca7e87348f1ea38944decc1f46f5db
Commit | Autor | Alter
4244cc 1 /************************************************************************
JS 2
3         POLHEMUS PROPRIETARY
4
5         Polhemus 
6         P.O. Box 560
7         Colchester, Vermont 05446
8         (802) 655-3159
9
10
11
12                 
13         Copyright © 2005 by Polhemus
14         All Rights Reserved.
15
16
17 *************************************************************************/
18
19 // Quaternion.cpp: implementation of the CQuaternion class.
20 //
21 //////////////////////////////////////////////////////////////////////
22
23 #include "Quaternion.h"
24 #include <math.h>
25
26
27 //////////////////////////////////////////////////////////////////////
28 // Construction/Destruction
29 //////////////////////////////////////////////////////////////////////
30
31 const float PI=3.14159265f;
32 const float DEG2RADS=PI/180.0f;
33
34
35
36 // Function name    : CQuaternion::CQuaternion
37 // Description        : Default Constructor -- Orientaton 0,0,0
38 // Return type        : 
39 CQuaternion::CQuaternion()
40 {
41     q0=1.0f;
42     q1=q2=q3=0.0f;
43 }
44
45 CQuaternion::~CQuaternion()
46 {
47
48 }
49
50
51 // Function name    : CQuaternion::CQuaternion
52 // Description        : Constructs class from array of four floats
53 // Argument         : float q[]
54 CQuaternion::CQuaternion(const float q[])
55 {
56     q0=q[0];
57     q1=q[1];
58     q2=q[2];
59     q3=q[3];
60     Normalize();
61
62 }
63
64
65 // Function name    : CQuaternion::CQuaternion
66 // Description        : Constructs class from 4 individual floats
67 // Argument         : float quat0
68 // Argument         : float quat1
69 // Argument         : float quat2
70 // Argument         : float quat3
71 CQuaternion::CQuaternion(float quat0, float quat1, float quat2, float quat3)
72 {
73     q0=quat0;
74     q1=quat1;
75     q2=quat2;
76     q3=quat3;
77     Normalize();
78 }
79
80
81 // Function name    : CQuaternion::CQuaternion
82 // Description        : Constructs class from az,el,roll in either deg or radians
83 // Argument         : float az
84 // Argument         : float el
85 // Argument         : float rl
86 // Argument         : bool deg - bool to indicate that angles are in degrees.  Default is true
87 CQuaternion::CQuaternion(float az, float el, float rl,bool deg)
88 {
89     float eul[3]={az,el,rl};
90     SetFromEulers(eul,deg);
91
92 }
93
94
95 // Function name    : CQuaternion::CQuaternion
96 // Description        : Constructs class from an axis and an angle of rotation about that axis
97 // Argument         : float vect[] - 3 values representing the vector which is the axis of rotation
98 // Argument         : float angle - The angle to rotate about vect.  Degrees or Radians
99 // Argument         : bool deg - bool to indicate that angles are in degrees.  Default is true
100 CQuaternion::CQuaternion(const float vect[], float angle, bool deg)
101 {
102     if (deg)
103         angle*=DEG2RADS;
104
105     float halfAng=angle/2.0f;
106
107     q0=(float)cos(halfAng);
108     q1=vect[0]*(float)sin(halfAng);
109     q2=vect[1]*(float)sin(halfAng);
110     q3=vect[2]*(float)sin(halfAng);
111
112     Normalize();
113 }
114
115
116
117 // Function name    : CQuaternion::GetEuler
118 // Description        : Returns the Euler angles associated with this quaternion in deg or rads
119 // Return type        : void 
120 // Argument         : float& az - azimuth returned here.
121 // Argument         : float& el - elevation returned here.
122 // Argument         : float& rl - roll returned here.
123 // Argument         : bool deg - If true returned angles are in degrees, otherwise radians.  Default is true
124 void CQuaternion::GetEuler(float& az, float& el, float& rl,bool deg) const
125 {
126     float mat[3][3];
127     float sinAz,sinEl,sinRl,cosAz,cosEl,cosRl;
128
129     // create orthogonal Attitude matrix
130     GetAttMat(mat);
131
132     sinEl=-mat[2][0];
133     cosEl=(float)sqrt(1.0f-(sinEl*sinEl));
134
135     if (fabs(cosEl)<0.001f){
136         sinAz=0.0f;
137         cosAz=1.0f;
138     }
139
140     else {
141         sinAz=mat[1][0]/cosEl;
142         cosAz=mat[0][0]/cosEl;
143     }
144
145     sinRl=sinAz*mat[0][2]-cosAz*mat[1][2];
146     cosRl=-sinAz*mat[0][1]+cosAz*mat[1][1];
147     
148     az=(float)atan2((double)sinAz,(double)cosAz);
149     el=(float)atan2((double)sinEl,(double)cosEl);
150     rl=(float)atan2((double)sinRl,(double)cosRl);
151
152     if (deg){            // convert to degrees
153         az/=DEG2RADS;
154         el/=DEG2RADS;
155         rl/=DEG2RADS;
156     }
157
158 }
159
160
161 // Function name    : CQuaternion::GetEuler
162 // Description        : Returns the Euler angles associated with this quaternion in deg or rads
163 // Return type        : void 
164 // Argument         : float aer[] - array of 3 floats where az,el,roll are returned.
165 // Argument         : bool deg - If true returned angles are in degrees, otherwise radians.  Default is true
166 void CQuaternion::GetEuler(float aer[],bool deg) const
167 {
168     GetEuler(aer[0],aer[1],aer[2],deg);
169 }
170
171
172 // Function name    : CQuaternion::Normalize
173 // Description        : normalizes the quat to a unit quaternion
174 // Return type        : void 
175 void CQuaternion::Normalize()
176 {
177     float mag=(float)sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
178
179     if (q0<0.0f)
180         mag*=-1.0f;        // make first entry pos
181     
182         q0/=mag;
183         q1/=mag;
184         q2/=mag;
185         q3/=mag;
186
187 }
188
189
190 // Function name    : CQuaternion::GetAttMat
191 // Description        : Returns the attitude matrix associated with this quaternion
192 // Return type        : void 
193 // Argument         : float mat[][3] - 3x3 float array where att matrix is returned.
194 void CQuaternion::GetAttMat(float mat[][3]) const
195 {
196     // create orthogonal Attitude matrix
197     mat[0][0]=q0*q0+q1*q1-q2*q2-q3*q3;
198     mat[0][1]=2*(q1*q2-q0*q3);
199     mat[0][2]=2*(q1*q3+q0*q2);
200     mat[1][0]=2*(q0*q3+q1*q2);
201     mat[1][1]=q0*q0-q1*q1+q2*q2-q3*q3;
202     mat[1][2]=2*(q2*q3-q0*q1);
203     mat[2][0]=2*(q1*q3-q0*q2);
204     mat[2][1]=2*(q0*q1+q2*q3);
205     mat[2][2]=q0*q0-q1*q1-q2*q2+q3*q3;
206
207 }
208
209
210
211
212
213
214 // Function name    : *
215 // Description        : multiplies two quaternion.
216 // Return type        : CQuaternion - The product of the multiplication. 
217 // Argument         : const CQuaternion& quat2 - The quat to mult this quat by.
218 CQuaternion CQuaternion::operator *(const CQuaternion& quat2) const
219 {
220
221     CQuaternion prod;
222
223     prod.q0 = q0*quat2.q0 - q1*quat2.q1 - q2*quat2.q2 - q3*quat2.q3;
224     prod.q1 = q0*quat2.q1 + q1*quat2.q0 + q2*quat2.q3 - q3*quat2.q2;
225     prod.q2 = q0*quat2.q2 - q1*quat2.q3 + q2*quat2.q0 + q3*quat2.q1;
226     prod.q3 = q0*quat2.q3 + q1*quat2.q2 - q2*quat2.q1 + q3*quat2.q0;
227     
228     prod.Normalize();
229     return prod;
230 }
231
232 // Function name    : *
233 // Description        : Multiplies quaternion by a scaler
234 // Return type        : CQuaternion -- the product 
235 // Argument         : const float scaler
236 CQuaternion CQuaternion::operator *(const float scaler) const
237 {
238     CQuaternion prod;
239     prod.q0=q0*scaler;
240     prod.q1=q1*scaler;
241     prod.q2=q2*scaler;
242     prod.q3=q3*scaler;
243
244     return prod;
245 }
246
247
248
249 // Function name    : *=
250 // Description        : Multiply and assign eg.  thisQuat*=thatQuat--->thisQuat=thisQuat*thatQuat
251 // Return type        : void  
252 // Argument         : const CQuaternion &quat - The quat to mult this quat by.
253 void CQuaternion::operator *=(const CQuaternion &quat)
254 {
255     CQuaternion prod=*this * quat;
256     *this=prod;
257 }
258
259
260 // Function name    : CQuaternion::GetAxisAngle
261 // Description        : Returns the axis and angle of rotation that is associated with this quaternion 
262 // Return type        : void 
263 // Argument         : float vect[] - array of 3 floats to receive vector representing the axis
264 // Argument         : float &angle - reference to received the angle.
265 // Argument         : bool deg - if true angle will be in degrees, otherwise radians.  Default is true.
266 void CQuaternion::GetAxisAngle(float vect[], float &angle, bool deg) const
267 {
268     angle=2.0f*(float)acos(q0);
269     if (deg)
270         angle/=DEG2RADS;
271
272     float scale=(float)sqrt(q1*q1+q2*q2+q3*q3);
273
274     if (fabs(scale)<0.001f){        // infinite axis, set to no rotation
275         angle=0.0f;
276         vect[0]=1.0f;
277         vect[1]=vect[2]=0.0f;
278         return;
279     }
280
281     vect[0]=q1/scale;
282     vect[1]=q2/scale;
283     vect[2]=q3/scale;
284
285     scale=(float)sqrt(vect[0]*vect[0]+vect[1]*vect[1]+vect[2]*vect[2]);
286     for (int i=0;i<3;i++)
287         vect[i]/=scale;
288 }
289
290
291 // Function name    : - 
292 // Description        : Returns inverse of quaternion.  eg -thisQuat --> InvthisQuat
293 // Return type        : CQuaternion - The inverse of this quaternion 
294 CQuaternion CQuaternion::operator -() const
295 {
296     CQuaternion inv=*this;
297     inv.q1*=-1;
298     inv.q2*=-1;
299     inv.q3*=-1;
300     return inv;
301 }
302
303
304
305 // Function name    : CQuaternion::GetDeltaQuat
306 // Description        : Returns the quaternion that represents the difference between two quaternions
307 // Return type        : CQuaternion - the delta quaternion
308 // Argument         : const CQuaternion &quat - reference to a quaternion to measure the difference between
309 CQuaternion CQuaternion::GetDeltaQuat(const CQuaternion &quat) const
310 {
311 /*    CQuaternion inv=-(*this);
312     return inv*quat;
313 */
314     CQuaternion inv=-quat;
315     return *this * inv;
316
317 }
318
319
320 // Function name    : - 
321 // Description        : Also retrieves the delta quat. eg.  thisQuat-thatQuat --> GetDeltaQuat(thatQuat)
322 //                      Yes this is really a multiplicaton process, but it's kind of intuitive.
323 // Return type        : CQuaternion CQuaternion::operator 
324 // Argument         : const CQuaternion &quat
325 CQuaternion CQuaternion::operator -(const CQuaternion &quat) const
326 {
327     return GetDeltaQuat(quat);
328 }
329
330
331 // Function name    : CQuaternion::GetQuatVal
332 // Description        : returns the four values of the quaternion
333 // Return type        : void 
334 // Argument         : float vals[]    - array of 4 floats to recieve the quat values
335 void CQuaternion::GetQuatVal(float vals[]) const
336 {
337     vals[0]=q0;
338     vals[1]=q1;
339     vals[2]=q2;
340     vals[3]=q3;
341     
342 }
343
344
345 // Function name    : CQuaternion::GetQuatVal
346 // Description        : Returns an individual value of the quaternion
347 // Return type        : float - the requested value of the quaternion
348 // Argument         : int ind - the index (0-3) of the value to return. Invalid values return q0.
349 float CQuaternion::GetQuatVal(int ind) const
350 {
351     float retVal;
352     switch (ind) {
353     case 0:
354     default:
355         retVal=q0;
356         break;
357     case 1:
358         retVal=q1;
359         break;
360     case 2:
361         retVal=q2;
362         break;
363     case 3:
364         retVal=q3;
365     }
366
367     return retVal;
368 }
369
370
371 // Function name    : CQuaternion::SetQuatVals
372 // Description        : Sets the values of the quaternion as indicated.
373 // Return type        : void 
374 // Argument         : float val[] - Array of 4 floats that indicate the values to set the quaternion members to.
375 void CQuaternion::SetQuatVals(const float val[])
376 {
377     SetQuatVals(val[0],val[1],val[2],val[3]);
378 }
379
380
381 // Function name    : CQuaternion::SetQuatVals
382 // Description        : Sets the values of the quaternion as indicated by the individual parameters
383 // Return type        : void 
384 // Argument         : float w    - set first quaternion value to this
385 // Argument         : float x    - set second quaternion value to this
386 // Argument         : float y    - set third quaternion value to this
387 // Argument         : float z    - set fourth quaternion value to this
388 void CQuaternion::SetQuatVals(const float w, const float x, const float y, const float z)
389 {
390     q0=w;
391     q1=x;
392     q2=y;
393     q3=z;
394     Normalize();
395
396 }
397
398 bool CQuaternion::IsIdentity()
399 {
400     return ((q0==1.0f) && (q1==0.0f) && (q2==0.0f) && (q3==0.0f));
401 }
402
403 void CQuaternion::Eul2Quat(float *quat, float *eul, bool isDeg)
404 {
405     CQuaternion q(eul[0],eul[1],eul[2],isDeg);
406     q.MakeLargestElementPos();
407     q.GetQuatVal(quat);
408 }
409
410 void CQuaternion::Quats2Eul(float *eul, float *quats, bool isDeg)
411 {
412     CQuaternion q(quats);
413     q.GetEuler(eul,isDeg);
414 }
415
416 void CQuaternion::MakeLargestElementPos()
417 {
418     float max=(float)fabs(q0);
419     bool bChgSign=q0<0;
420
421
422     if ((float)fabs(q1)>max){
423         max=(float)fabs(q1);
424         bChgSign=q1<0;
425     }
426
427     if ((float)fabs(q2)>max){
428         max=(float)fabs(q2);
429         bChgSign=q2<0;
430     }
431
432     if ((float)fabs(q3)>max){
433         max=(float)fabs(q3);
434         bChgSign=q3<0;
435     }
436
437     if (bChgSign){
438         q0*=-1;
439         q1*=-1;
440         q2*=-1;
441         q3*=-1;
442     }
443 }
444
445 void CQuaternion::SetFromEulers(float *eul, bool deg/*=true*/)
446 {
447     float azHalf=eul[0]/2.0f;
448     float elHalf=eul[1]/2.0f;
449     float rollHalf=eul[2]/2.0f;
450
451     if (deg){
452         azHalf*=DEG2RADS;
453         elHalf*=DEG2RADS;
454         rollHalf*=DEG2RADS;
455     }
456
457     q0=(float)(cos(azHalf)*cos(elHalf)*cos(rollHalf)+sin(azHalf)*sin(elHalf)*sin(rollHalf));
458     q1=(float)(cos(azHalf)*cos(elHalf)*sin(rollHalf)-sin(azHalf)*sin(elHalf)*cos(rollHalf));
459     q2=(float)(cos(azHalf)*sin(elHalf)*cos(rollHalf)+sin(azHalf)*cos(elHalf)*sin(rollHalf));
460     q3=(float)(sin(azHalf)*cos(elHalf)*cos(rollHalf)-cos(azHalf)*sin(elHalf)*sin(rollHalf));
461
462
463     Normalize();
464     
465 }
466
467 // Function name    : CQuaternion::Slerp
468 // Description        : Spherical interpolation of quaternions.  Interpolates between
469 //                        this quaternion and the quaternion passed in as a parameter
470 // Return type        : CQuaternion -- The interpolated quaternion
471 // Argument         : const CQuaternion &q
472 // Argument         : float t -- value between 0 and 1 which indicates the distance between 
473 //                        quaternions to interpolate
474 CQuaternion CQuaternion::Slerp(const CQuaternion &q, float t) const
475 {
476
477     //    qa*sin((1-t)theta)+qb*sin(t*theta)
478     // q= ----------------------------------
479     //                sin(theta)
480
481     // 2*theta = qa.q0*qb.q0+qa.q1*qb.q1+qa.q2*qb.q2+qa.q3*qb.q3
482
483
484
485
486     float angle;
487
488     if (t>=1.0f)
489         return CQuaternion(q);
490
491     if (t<=0.0f)
492         return CQuaternion(*this);
493
494
495     float dot=q0*q.q0+q1*q.q1+q2*q.q2+q3*q.q3;
496     angle=(float)(acos(dot))/2.0f;
497     if (angle<0.0f)
498         angle*=-1;
499
500     float coeff1,coeff2;
501
502     if (angle==0.0f){
503         coeff1=1-t;
504         coeff2=t;
505     }
506     else {
507
508         coeff1=(float)sin((1.0f-t)*angle)/(float)sin(angle);
509         coeff2=(float)sin(t*angle)/(float)sin(angle);
510     }
511
512     CQuaternion res=*this*coeff1+q*coeff2;
513     res.Normalize();
514
515     return res;
516 }
517
518 // Function name    : +
519 // Description        : Adds to quaternions
520 // Return type        : CQuaternion -- the sum
521 // Argument         : const CQuaternion &q1
522 // Argument         : const CQuaternion &q2
523 CQuaternion CQuaternion::operator +(const CQuaternion &q) const
524 {
525     CQuaternion sum;
526     sum.q0=q0+q.q0;
527     sum.q1=q1+q.q1;
528     sum.q2=q2+q.q2;
529     sum.q3=q3+q.q3;
530
531     return sum;
532
533 }
534
535
536
537
538 void CQuaternion::GetAngle(float &a,bool deg)
539 {
540     a=2.0f*(float)acos(q0);
541     if (deg)
542         a/=DEG2RADS;
543
544 }