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