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 |
}
|