1 | #ifndef NX_FOUNDATION_NxQuatT
|
---|
2 | #define NX_FOUNDATION_NxQuatT
|
---|
3 | /*----------------------------------------------------------------------------*\
|
---|
4 | |
|
---|
5 | | Public Interface to NovodeX Technology
|
---|
6 | |
|
---|
7 | | www.novodex.com
|
---|
8 | |
|
---|
9 | \*----------------------------------------------------------------------------*/
|
---|
10 | /** \addtogroup foundation
|
---|
11 | @{
|
---|
12 | */
|
---|
13 |
|
---|
14 | #include "Nxf.h"
|
---|
15 | #include "NxVec3.h"
|
---|
16 |
|
---|
17 | /**
|
---|
18 | \brief This is a quaternion class.
|
---|
19 |
|
---|
20 | */
|
---|
21 |
|
---|
22 | class NxQuat
|
---|
23 | {
|
---|
24 | public:
|
---|
25 | /**
|
---|
26 | \brief Default constructor, does not do any initialization.
|
---|
27 | */
|
---|
28 | NX_INLINE NxQuat();
|
---|
29 |
|
---|
30 | /**
|
---|
31 | \brief Copy constructor.
|
---|
32 | */
|
---|
33 | NX_INLINE NxQuat(const NxQuat&);
|
---|
34 |
|
---|
35 | /**
|
---|
36 | \brief copies xyz elements from v, and scalar from w (defaults to 0).
|
---|
37 | */
|
---|
38 | NX_INLINE NxQuat(const NxVec3& v, NxReal w = 0);
|
---|
39 |
|
---|
40 | /**
|
---|
41 | \brief creates from angle-axis representation.
|
---|
42 |
|
---|
43 | note that if Angle > 360 the resulting rotation is Angle mod 360.
|
---|
44 |
|
---|
45 | <b>Unit:</b> Degrees
|
---|
46 | */
|
---|
47 | NX_INLINE NxQuat(const NxReal angle, const NxVec3 & axis);
|
---|
48 |
|
---|
49 | /**
|
---|
50 | \brief Creates from orientation matrix.
|
---|
51 |
|
---|
52 | \param[in] m Rotation matrix to extract quaterion from.
|
---|
53 | */
|
---|
54 | NX_INLINE NxQuat(const class NxMat33 &m); /* defined in NxMat33.h */
|
---|
55 |
|
---|
56 |
|
---|
57 | /**
|
---|
58 | \brief Set the quaternion to the identity rotation.
|
---|
59 | */
|
---|
60 | NX_INLINE void id();
|
---|
61 |
|
---|
62 | /**
|
---|
63 | \brief Test if the quaterion is the identity rotation.
|
---|
64 | */
|
---|
65 | NX_INLINE bool isIdentityRotation() const;
|
---|
66 |
|
---|
67 | //setting:
|
---|
68 |
|
---|
69 | /**
|
---|
70 | \brief Set the members of the quaterion, in order WXYZ
|
---|
71 | */
|
---|
72 | NX_INLINE void setWXYZ(NxReal w, NxReal x, NxReal y, NxReal z);
|
---|
73 |
|
---|
74 | /**
|
---|
75 | \brief Set the members of the quaterion, in order XYZW
|
---|
76 | */
|
---|
77 | NX_INLINE void setXYZW(NxReal x, NxReal y, NxReal z, NxReal w);
|
---|
78 |
|
---|
79 | /**
|
---|
80 | \brief Set the members of the quaterion, in order WXYZ
|
---|
81 | */
|
---|
82 | NX_INLINE void setWXYZ(const NxReal *);
|
---|
83 |
|
---|
84 | /**
|
---|
85 | \brief Set the members of the quaterion, in order XYZW
|
---|
86 | */
|
---|
87 | NX_INLINE void setXYZW(const NxReal *);
|
---|
88 |
|
---|
89 | NX_INLINE NxQuat& operator= (const NxQuat&);
|
---|
90 |
|
---|
91 | /**
|
---|
92 | \brief Implicitly extends vector by a 0 w element.
|
---|
93 | */
|
---|
94 | NX_INLINE NxQuat& operator= (const NxVec3&);
|
---|
95 |
|
---|
96 | NX_INLINE void setx(const NxReal& d);
|
---|
97 | NX_INLINE void sety(const NxReal& d);
|
---|
98 | NX_INLINE void setz(const NxReal& d);
|
---|
99 | NX_INLINE void setw(const NxReal& d);
|
---|
100 |
|
---|
101 | NX_INLINE void getWXYZ(NxF32 *) const;
|
---|
102 | NX_INLINE void getXYZW(NxF32 *) const;
|
---|
103 |
|
---|
104 | NX_INLINE void getWXYZ(NxF64 *) const;
|
---|
105 | NX_INLINE void getXYZW(NxF64 *) const;
|
---|
106 |
|
---|
107 | /**
|
---|
108 | \brief returns true if all elems are finite (not NAN or INF, etc.)
|
---|
109 | */
|
---|
110 | NX_INLINE bool isFinite() const;
|
---|
111 |
|
---|
112 | /**
|
---|
113 | \brief sets to the quat [0,0,0,1]
|
---|
114 | */
|
---|
115 | NX_INLINE void zero();
|
---|
116 |
|
---|
117 | /**
|
---|
118 | \brief creates a random unit quaternion.
|
---|
119 | */
|
---|
120 | NX_INLINE void random();
|
---|
121 | /**
|
---|
122 | \brief creates from angle-axis representation.
|
---|
123 |
|
---|
124 | Note that if Angle > 360 the resulting rotation is Angle mod 360.
|
---|
125 |
|
---|
126 | <b>Unit:</b> Degrees
|
---|
127 | */
|
---|
128 | NX_INLINE void fromAngleAxis(NxReal angle, const NxVec3 & axis);
|
---|
129 |
|
---|
130 | /**
|
---|
131 | \brief Creates from angle-axis representation.
|
---|
132 |
|
---|
133 | Axis must be normalized!
|
---|
134 |
|
---|
135 | <b>Unit:</b> Radians
|
---|
136 | */
|
---|
137 | NX_INLINE void fromAngleAxisFast(NxReal AngleRadians, const NxVec3 & axis);
|
---|
138 |
|
---|
139 | /**
|
---|
140 | \brief Sets this to the opposite rotation of this.
|
---|
141 | */
|
---|
142 | NX_INLINE void invert();
|
---|
143 |
|
---|
144 | /**
|
---|
145 | \brief Fetches the Angle/axis given by the NxQuat.
|
---|
146 |
|
---|
147 | <b>Unit:</b> Degrees
|
---|
148 | */
|
---|
149 | NX_INLINE void getAngleAxis(NxReal& Angle, NxVec3 & axis) const;
|
---|
150 |
|
---|
151 | /**
|
---|
152 | \brief Gets the angle between this quat and the identity quaternion.
|
---|
153 |
|
---|
154 | <b>Unit:</b> Degrees
|
---|
155 | */
|
---|
156 | NX_INLINE NxReal getAngle() const;
|
---|
157 |
|
---|
158 | /**
|
---|
159 | \brief Gets the angle between this quat and the argument
|
---|
160 |
|
---|
161 | <b>Unit:</b> Degrees
|
---|
162 | */
|
---|
163 | NX_INLINE NxReal getAngle(const NxQuat &) const;
|
---|
164 |
|
---|
165 | /**
|
---|
166 | \brief This is the squared 4D vector length, should be 1 for unit quaternions.
|
---|
167 | */
|
---|
168 | NX_INLINE NxReal magnitudeSquared() const;
|
---|
169 |
|
---|
170 | /**
|
---|
171 | \brief returns the scalar product of this and other.
|
---|
172 | */
|
---|
173 | NX_INLINE NxReal dot(const NxQuat &other) const;
|
---|
174 |
|
---|
175 | //modifiers:
|
---|
176 | /**
|
---|
177 | \brief maps to the closest unit quaternion.
|
---|
178 | */
|
---|
179 | NX_INLINE void normalize();
|
---|
180 |
|
---|
181 | /*
|
---|
182 | \brief assigns its own conjugate to itself.
|
---|
183 |
|
---|
184 | \note for unit quaternions, this is the inverse.
|
---|
185 | */
|
---|
186 | NX_INLINE void conjugate();
|
---|
187 |
|
---|
188 | /**
|
---|
189 | this = a * b
|
---|
190 | */
|
---|
191 | NX_INLINE void multiply(const NxQuat& a, const NxQuat& b);
|
---|
192 |
|
---|
193 | /**
|
---|
194 | this = a * v
|
---|
195 | v is interpreted as quat [xyz0]
|
---|
196 | */
|
---|
197 | NX_INLINE void multiply(const NxQuat& a, const NxVec3& v);
|
---|
198 |
|
---|
199 | /**
|
---|
200 | this = slerp(t, a, b)
|
---|
201 | */
|
---|
202 | NX_INLINE void slerp(const NxReal t, const NxQuat& a, const NxQuat& b);
|
---|
203 |
|
---|
204 | /**
|
---|
205 | rotates passed vec by rot expressed by unit quaternion. overwrites arg ith the result.
|
---|
206 | */
|
---|
207 | NX_INLINE void rotate(NxVec3 &) const;
|
---|
208 |
|
---|
209 | /**
|
---|
210 | rotates passed vec by this (assumed unitary)
|
---|
211 | */
|
---|
212 | NX_INLINE const NxVec3 rot(const NxVec3 &) const;
|
---|
213 |
|
---|
214 | /**
|
---|
215 | inverse rotates passed vec by this (assumed unitary)
|
---|
216 | */
|
---|
217 | NX_INLINE const NxVec3 invRot(const NxVec3 &) const;
|
---|
218 |
|
---|
219 | /**
|
---|
220 | transform passed vec by this rotation (assumed unitary) and translation p
|
---|
221 | */
|
---|
222 | NX_INLINE const NxVec3 transform(const NxVec3 &v, const NxVec3 &p) const;
|
---|
223 |
|
---|
224 | /**
|
---|
225 | inverse rotates passed vec by this (assumed unitary)
|
---|
226 | */
|
---|
227 | NX_INLINE const NxVec3 invTransform(const NxVec3 &v, const NxVec3 &p) const;
|
---|
228 |
|
---|
229 |
|
---|
230 | /**
|
---|
231 | rotates passed vec by opposite of rot expressed by unit quaternion. overwrites arg ith the result.
|
---|
232 | */
|
---|
233 | NX_INLINE void inverseRotate(NxVec3 &) const;
|
---|
234 |
|
---|
235 |
|
---|
236 |
|
---|
237 | /**
|
---|
238 | negates all the elements of the quat. q and -q represent the same rotation.
|
---|
239 | */
|
---|
240 | NX_INLINE void negate();
|
---|
241 | NX_INLINE NxQuat operator -() const;
|
---|
242 |
|
---|
243 | NX_INLINE NxQuat& operator*= (const NxQuat&);
|
---|
244 | NX_INLINE NxQuat& operator+= (const NxQuat&);
|
---|
245 | NX_INLINE NxQuat& operator-= (const NxQuat&);
|
---|
246 | NX_INLINE NxQuat& operator*= (const NxReal s);
|
---|
247 |
|
---|
248 | NxReal x,y,z,w;
|
---|
249 |
|
---|
250 | /** quaternion multiplication */
|
---|
251 | NX_INLINE NxQuat operator *(const NxQuat &) const;
|
---|
252 |
|
---|
253 | /** quaternion addition */
|
---|
254 | NX_INLINE NxQuat operator +(const NxQuat &) const;
|
---|
255 |
|
---|
256 | /** quaternion subtraction */
|
---|
257 | NX_INLINE NxQuat operator -(const NxQuat &) const;
|
---|
258 |
|
---|
259 | /** quaternion conjugate */
|
---|
260 | NX_INLINE NxQuat operator !() const;
|
---|
261 |
|
---|
262 | /*
|
---|
263 | ops we decided not to implement:
|
---|
264 | bool operator== (const NxQuat&) const;
|
---|
265 | NxVec3 operator^ (const NxQuat& r_h_s) const;//same as normal quat rot, but casts itself into a vector. (doesn't compute w term)
|
---|
266 | NxQuat operator* (const NxVec3& v) const;//implicitly extends vector by a 0 w element.
|
---|
267 | NxQuat operator* (const NxReal Scale) const;
|
---|
268 | */
|
---|
269 |
|
---|
270 | friend class NxMat33;
|
---|
271 | private:
|
---|
272 | NX_INLINE NxQuat(NxReal ix, NxReal iy, NxReal iz, NxReal iw);
|
---|
273 | };
|
---|
274 |
|
---|
275 |
|
---|
276 |
|
---|
277 |
|
---|
278 | NX_INLINE NxQuat::NxQuat()
|
---|
279 | {
|
---|
280 | //nothing
|
---|
281 | }
|
---|
282 |
|
---|
283 |
|
---|
284 | NX_INLINE NxQuat::NxQuat(const NxQuat& q) : x(q.x), y(q.y), z(q.z), w(q.w)
|
---|
285 | {
|
---|
286 | }
|
---|
287 |
|
---|
288 |
|
---|
289 | NX_INLINE NxQuat::NxQuat(const NxVec3& v, NxReal s) // copy constructor, assumes w=0
|
---|
290 | {
|
---|
291 | x = v.x;
|
---|
292 | y = v.y;
|
---|
293 | z = v.z;
|
---|
294 | w = s;
|
---|
295 | }
|
---|
296 |
|
---|
297 |
|
---|
298 | NX_INLINE NxQuat::NxQuat(const NxReal angle, const NxVec3 & axis) // creates a NxQuat from an Angle axis -- note that if Angle > 360 the resulting rotation is Angle mod 360
|
---|
299 | {
|
---|
300 | fromAngleAxis(angle,axis);
|
---|
301 | }
|
---|
302 |
|
---|
303 |
|
---|
304 | NX_INLINE void NxQuat::id()
|
---|
305 | {
|
---|
306 | x = NxReal(0);
|
---|
307 | y = NxReal(0);
|
---|
308 | z = NxReal(0);
|
---|
309 | w = NxReal(1);
|
---|
310 | }
|
---|
311 |
|
---|
312 | NX_INLINE bool NxQuat::isIdentityRotation() const
|
---|
313 | {
|
---|
314 | return x==0 && y==0 && z==0 && fabsf(w)==1;
|
---|
315 | }
|
---|
316 |
|
---|
317 |
|
---|
318 | NX_INLINE void NxQuat::setWXYZ(NxReal sw, NxReal sx, NxReal sy, NxReal sz)
|
---|
319 | {
|
---|
320 | x = sx;
|
---|
321 | y = sy;
|
---|
322 | z = sz;
|
---|
323 | w = sw;
|
---|
324 | }
|
---|
325 |
|
---|
326 |
|
---|
327 | NX_INLINE void NxQuat::setXYZW(NxReal sx, NxReal sy, NxReal sz, NxReal sw)
|
---|
328 | {
|
---|
329 | x = sx;
|
---|
330 | y = sy;
|
---|
331 | z = sz;
|
---|
332 | w = sw;
|
---|
333 | }
|
---|
334 |
|
---|
335 |
|
---|
336 | NX_INLINE void NxQuat::setWXYZ(const NxReal * d)
|
---|
337 | {
|
---|
338 | x = d[1];
|
---|
339 | y = d[2];
|
---|
340 | z = d[3];
|
---|
341 | w = d[0];
|
---|
342 | }
|
---|
343 |
|
---|
344 |
|
---|
345 | NX_INLINE void NxQuat::setXYZW(const NxReal * d)
|
---|
346 | {
|
---|
347 | x = d[0];
|
---|
348 | y = d[1];
|
---|
349 | z = d[2];
|
---|
350 | w = d[3];
|
---|
351 | }
|
---|
352 |
|
---|
353 |
|
---|
354 | NX_INLINE void NxQuat::getWXYZ(NxF32 *d) const
|
---|
355 | {
|
---|
356 | d[1] = (NxF32)x;
|
---|
357 | d[2] = (NxF32)y;
|
---|
358 | d[3] = (NxF32)z;
|
---|
359 | d[0] = (NxF32)w;
|
---|
360 | }
|
---|
361 |
|
---|
362 |
|
---|
363 | NX_INLINE void NxQuat::getXYZW(NxF32 *d) const
|
---|
364 | {
|
---|
365 | d[0] = (NxF32)x;
|
---|
366 | d[1] = (NxF32)y;
|
---|
367 | d[2] = (NxF32)z;
|
---|
368 | d[3] = (NxF32)w;
|
---|
369 | }
|
---|
370 |
|
---|
371 |
|
---|
372 | NX_INLINE void NxQuat::getWXYZ(NxF64 *d) const
|
---|
373 | {
|
---|
374 | d[1] = (NxF64)x;
|
---|
375 | d[2] = (NxF64)y;
|
---|
376 | d[3] = (NxF64)z;
|
---|
377 | d[0] = (NxF64)w;
|
---|
378 | }
|
---|
379 |
|
---|
380 |
|
---|
381 | NX_INLINE void NxQuat::getXYZW(NxF64 *d) const
|
---|
382 | {
|
---|
383 | d[0] = (NxF64)x;
|
---|
384 | d[1] = (NxF64)y;
|
---|
385 | d[2] = (NxF64)z;
|
---|
386 | d[3] = (NxF64)w;
|
---|
387 | }
|
---|
388 |
|
---|
389 | //const methods
|
---|
390 |
|
---|
391 | NX_INLINE bool NxQuat::isFinite() const
|
---|
392 | {
|
---|
393 | return NxMath::isFinite(x)
|
---|
394 | && NxMath::isFinite(y)
|
---|
395 | && NxMath::isFinite(z)
|
---|
396 | && NxMath::isFinite(w);
|
---|
397 | }
|
---|
398 |
|
---|
399 |
|
---|
400 |
|
---|
401 | NX_INLINE void NxQuat::zero()
|
---|
402 | {
|
---|
403 | x = NxReal(0.0);
|
---|
404 | y = NxReal(0.0);
|
---|
405 | z = NxReal(0.0);
|
---|
406 | w = NxReal(1.0);
|
---|
407 | }
|
---|
408 |
|
---|
409 |
|
---|
410 | NX_INLINE void NxQuat::negate()
|
---|
411 | {
|
---|
412 | x = -x;
|
---|
413 | y = -y;
|
---|
414 | z = -z;
|
---|
415 | w = -w;
|
---|
416 | }
|
---|
417 |
|
---|
418 | NX_INLINE NxQuat NxQuat::operator-() const
|
---|
419 | {
|
---|
420 | return NxQuat(-x,-y,-z,-w);
|
---|
421 | }
|
---|
422 |
|
---|
423 |
|
---|
424 | NX_INLINE void NxQuat::random()
|
---|
425 | {
|
---|
426 | x = NxMath::rand(NxReal(0.0),NxReal(1.0));
|
---|
427 | y = NxMath::rand(NxReal(0.0),NxReal(1.0));
|
---|
428 | z = NxMath::rand(NxReal(0.0),NxReal(1.0));
|
---|
429 | w = NxMath::rand(NxReal(0.0),NxReal(1.0));
|
---|
430 | normalize();
|
---|
431 | }
|
---|
432 |
|
---|
433 |
|
---|
434 | NX_INLINE void NxQuat::fromAngleAxis(NxReal Angle, const NxVec3 & axis) // set the NxQuat by Angle-axis (see AA constructor)
|
---|
435 | {
|
---|
436 | x = axis.x;
|
---|
437 | y = axis.y;
|
---|
438 | z = axis.z;
|
---|
439 |
|
---|
440 | // required: Normalize the axis
|
---|
441 |
|
---|
442 | const NxReal i_length = NxReal(1.0) / NxMath::sqrt( x*x + y*y + z*z );
|
---|
443 |
|
---|
444 | x = x * i_length;
|
---|
445 | y = y * i_length;
|
---|
446 | z = z * i_length;
|
---|
447 |
|
---|
448 | // now make a clQuaternionernion out of it
|
---|
449 | NxReal Half = NxMath::degToRad(Angle * NxReal(0.5));
|
---|
450 |
|
---|
451 | w = NxMath::cos(Half);//this used to be w/o deg to rad.
|
---|
452 | const NxReal sin_theta_over_two = NxMath::sin(Half );
|
---|
453 | x = x * sin_theta_over_two;
|
---|
454 | y = y * sin_theta_over_two;
|
---|
455 | z = z * sin_theta_over_two;
|
---|
456 | }
|
---|
457 |
|
---|
458 | NX_INLINE void NxQuat::fromAngleAxisFast(NxReal AngleRadians, const NxVec3 & axis)
|
---|
459 | {
|
---|
460 | NxReal s;
|
---|
461 | NxMath::sinCos(AngleRadians * 0.5f, s, w);
|
---|
462 | x = axis.x * s;
|
---|
463 | y = axis.y * s;
|
---|
464 | z = axis.z * s;
|
---|
465 | }
|
---|
466 |
|
---|
467 | NX_INLINE void NxQuat::invert()
|
---|
468 | {
|
---|
469 | x = -x;
|
---|
470 | y = -y;
|
---|
471 | z = -z;
|
---|
472 | }
|
---|
473 |
|
---|
474 | NX_INLINE void NxQuat::setx(const NxReal& d)
|
---|
475 | {
|
---|
476 | x = d;
|
---|
477 | }
|
---|
478 |
|
---|
479 |
|
---|
480 | NX_INLINE void NxQuat::sety(const NxReal& d)
|
---|
481 | {
|
---|
482 | y = d;
|
---|
483 | }
|
---|
484 |
|
---|
485 |
|
---|
486 | NX_INLINE void NxQuat::setz(const NxReal& d)
|
---|
487 | {
|
---|
488 | z = d;
|
---|
489 | }
|
---|
490 |
|
---|
491 |
|
---|
492 | NX_INLINE void NxQuat::setw(const NxReal& d)
|
---|
493 | {
|
---|
494 | w = d;
|
---|
495 | }
|
---|
496 |
|
---|
497 |
|
---|
498 | NX_INLINE void NxQuat::getAngleAxis(NxReal& angle, NxVec3 & axis) const
|
---|
499 | {
|
---|
500 | //return axis and angle of rotation of quaternion
|
---|
501 | angle = NxMath::acos(w) * NxReal(2.0); //this is getAngle()
|
---|
502 | NxReal sa = NxMath::sqrt(NxReal(1.0) - w*w);
|
---|
503 | if (sa)
|
---|
504 | {
|
---|
505 | axis.set(x/sa,y/sa,z/sa);
|
---|
506 | angle = NxMath::radToDeg(angle);
|
---|
507 | }
|
---|
508 | else
|
---|
509 | axis.zero();
|
---|
510 |
|
---|
511 | }
|
---|
512 |
|
---|
513 |
|
---|
514 |
|
---|
515 | NX_INLINE NxReal NxQuat::getAngle() const
|
---|
516 | {
|
---|
517 | return NxMath::acos(w) * NxReal(2.0);
|
---|
518 | }
|
---|
519 |
|
---|
520 |
|
---|
521 |
|
---|
522 | NX_INLINE NxReal NxQuat::getAngle(const NxQuat & q) const
|
---|
523 | {
|
---|
524 | return NxMath::acos(dot(q)) * NxReal(2.0);
|
---|
525 | }
|
---|
526 |
|
---|
527 |
|
---|
528 | NX_INLINE NxReal NxQuat::magnitudeSquared() const
|
---|
529 |
|
---|
530 | //modifiers:
|
---|
531 | {
|
---|
532 | return x*x + y*y + z*z + w*w;
|
---|
533 | }
|
---|
534 |
|
---|
535 |
|
---|
536 | NX_INLINE NxReal NxQuat::dot(const NxQuat &v) const
|
---|
537 | {
|
---|
538 | return x * v.x + y * v.y + z * v.z + w * v.w;
|
---|
539 | }
|
---|
540 |
|
---|
541 |
|
---|
542 | NX_INLINE void NxQuat::normalize() // convert this NxQuat to a unit clQuaternionernion
|
---|
543 | {
|
---|
544 | const NxReal mag = NxMath::sqrt(magnitudeSquared());
|
---|
545 | if (mag)
|
---|
546 | {
|
---|
547 | const NxReal imag = NxReal(1.0) / mag;
|
---|
548 |
|
---|
549 | x *= imag;
|
---|
550 | y *= imag;
|
---|
551 | z *= imag;
|
---|
552 | w *= imag;
|
---|
553 | }
|
---|
554 | }
|
---|
555 |
|
---|
556 |
|
---|
557 | NX_INLINE void NxQuat::conjugate() // convert this NxQuat to a unit clQuaternionernion
|
---|
558 | {
|
---|
559 | x = -x;
|
---|
560 | y = -y;
|
---|
561 | z = -z;
|
---|
562 | }
|
---|
563 |
|
---|
564 |
|
---|
565 | NX_INLINE void NxQuat::multiply(const NxQuat& left, const NxQuat& right) // this = a * b
|
---|
566 | {
|
---|
567 | NxReal a,b,c,d;
|
---|
568 |
|
---|
569 | a =left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
|
---|
570 | b =left.w*right.x + right.w*left.x + left.y*right.z - right.y*left.z;
|
---|
571 | c =left.w*right.y + right.w*left.y + left.z*right.x - right.z*left.x;
|
---|
572 | d =left.w*right.z + right.w*left.z + left.x*right.y - right.x*left.y;
|
---|
573 |
|
---|
574 | w = a;
|
---|
575 | x = b;
|
---|
576 | y = c;
|
---|
577 | z = d;
|
---|
578 | }
|
---|
579 |
|
---|
580 |
|
---|
581 | NX_INLINE void NxQuat::multiply(const NxQuat& left, const NxVec3& right) // this = a * b
|
---|
582 | {
|
---|
583 | NxReal a,b,c,d;
|
---|
584 |
|
---|
585 | a = - left.x*right.x - left.y*right.y - left.z *right.z;
|
---|
586 | b = left.w*right.x + left.y*right.z - right.y*left.z;
|
---|
587 | c = left.w*right.y + left.z*right.x - right.z*left.x;
|
---|
588 | d = left.w*right.z + left.x*right.y - right.x*left.y;
|
---|
589 |
|
---|
590 | w = a;
|
---|
591 | x = b;
|
---|
592 | y = c;
|
---|
593 | z = d;
|
---|
594 | }
|
---|
595 |
|
---|
596 | NX_INLINE void NxQuat::slerp(const NxReal t, const NxQuat& left, const NxQuat& right) // this = slerp(t, a, b)
|
---|
597 | {
|
---|
598 | const NxReal quatEpsilon = (NxReal(1.0e-8f));
|
---|
599 |
|
---|
600 | *this = left;
|
---|
601 |
|
---|
602 | NxReal cosine =
|
---|
603 | x * right.x +
|
---|
604 | y * right.y +
|
---|
605 | z * right.z +
|
---|
606 | w * right.w; //this is left.dot(right)
|
---|
607 |
|
---|
608 | NxReal sign = NxReal(1);
|
---|
609 | if (cosine < 0)
|
---|
610 | {
|
---|
611 | cosine = - cosine;
|
---|
612 | sign = NxReal(-1);
|
---|
613 | }
|
---|
614 |
|
---|
615 | NxReal Sin = NxReal(1) - cosine*cosine;
|
---|
616 |
|
---|
617 | if(Sin>=quatEpsilon*quatEpsilon)
|
---|
618 | {
|
---|
619 | Sin = NxMath::sqrt(Sin);
|
---|
620 | const NxReal angle = NxMath::atan2(Sin, cosine);
|
---|
621 | const NxReal i_sin_angle = NxReal(1) / Sin;
|
---|
622 |
|
---|
623 |
|
---|
624 |
|
---|
625 | NxReal lower_weight = NxMath::sin(angle*(NxReal(1)-t)) * i_sin_angle;
|
---|
626 | NxReal upper_weight = NxMath::sin(angle * t) * i_sin_angle * sign;
|
---|
627 |
|
---|
628 | w = (w * (lower_weight)) + (right.w * (upper_weight));
|
---|
629 | x = (x * (lower_weight)) + (right.x * (upper_weight));
|
---|
630 | y = (y * (lower_weight)) + (right.y * (upper_weight));
|
---|
631 | z = (z * (lower_weight)) + (right.z * (upper_weight));
|
---|
632 | }
|
---|
633 | }
|
---|
634 |
|
---|
635 |
|
---|
636 | NX_INLINE void NxQuat::rotate(NxVec3 & v) const //rotates passed vec by rot expressed by quaternion. overwrites arg ith the result.
|
---|
637 | {
|
---|
638 | //NxReal msq = NxReal(1.0)/magnitudeSquared(); //assume unit quat!
|
---|
639 | NxQuat myInverse;
|
---|
640 | myInverse.x = -x;//*msq;
|
---|
641 | myInverse.y = -y;//*msq;
|
---|
642 | myInverse.z = -z;//*msq;
|
---|
643 | myInverse.w = w;//*msq;
|
---|
644 |
|
---|
645 | //v = ((*this) * v) ^ myInverse;
|
---|
646 |
|
---|
647 | NxQuat left;
|
---|
648 | left.multiply(*this,v);
|
---|
649 | v.x =left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
|
---|
650 | v.y =left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
|
---|
651 | v.z =left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
|
---|
652 | }
|
---|
653 |
|
---|
654 |
|
---|
655 | NX_INLINE void NxQuat::inverseRotate(NxVec3 & v) const //rotates passed vec by opposite of rot expressed by quaternion. overwrites arg ith the result.
|
---|
656 | {
|
---|
657 | //NxReal msq = NxReal(1.0)/magnitudeSquared(); //assume unit quat!
|
---|
658 | NxQuat myInverse;
|
---|
659 | myInverse.x = -x;//*msq;
|
---|
660 | myInverse.y = -y;//*msq;
|
---|
661 | myInverse.z = -z;//*msq;
|
---|
662 | myInverse.w = w;//*msq;
|
---|
663 |
|
---|
664 | //v = (myInverse * v) ^ (*this);
|
---|
665 | NxQuat left;
|
---|
666 | left.multiply(myInverse,v);
|
---|
667 | v.x =left.w*x + w*left.x + left.y*z - y*left.z;
|
---|
668 | v.y =left.w*y + w*left.y + left.z*x - z*left.x;
|
---|
669 | v.z =left.w*z + w*left.z + left.x*y - x*left.y;
|
---|
670 | }
|
---|
671 |
|
---|
672 |
|
---|
673 | NX_INLINE NxQuat& NxQuat::operator= (const NxQuat& q)
|
---|
674 | {
|
---|
675 | x = q.x;
|
---|
676 | y = q.y;
|
---|
677 | z = q.z;
|
---|
678 | w = q.w;
|
---|
679 | return *this;
|
---|
680 | }
|
---|
681 |
|
---|
682 | #if 0
|
---|
683 | NX_INLINE NxQuat& NxQuat::operator= (const NxVec3& v) //implicitly extends vector by a 0 w element.
|
---|
684 | {
|
---|
685 | x = v.x;
|
---|
686 | y = v.y;
|
---|
687 | z = v.z;
|
---|
688 | w = NxReal(1.0);
|
---|
689 | return *this;
|
---|
690 | }
|
---|
691 | #endif
|
---|
692 |
|
---|
693 | NX_INLINE NxQuat& NxQuat::operator*= (const NxQuat& q)
|
---|
694 | {
|
---|
695 | NxReal xx[4]; //working Quaternion
|
---|
696 | xx[0] = w*q.w - q.x*x - y*q.y - q.z*z;
|
---|
697 | xx[1] = w*q.x + q.w*x + y*q.z - q.y*z;
|
---|
698 | xx[2] = w*q.y + q.w*y + z*q.x - q.z*x;
|
---|
699 | z=w*q.z + q.w*z + x*q.y - q.x*y;
|
---|
700 |
|
---|
701 | w = xx[0];
|
---|
702 | x = xx[1];
|
---|
703 | y = xx[2];
|
---|
704 | return *this;
|
---|
705 | }
|
---|
706 |
|
---|
707 |
|
---|
708 | NX_INLINE NxQuat& NxQuat::operator+= (const NxQuat& q)
|
---|
709 | {
|
---|
710 | x+=q.x;
|
---|
711 | y+=q.y;
|
---|
712 | z+=q.z;
|
---|
713 | w+=q.w;
|
---|
714 | return *this;
|
---|
715 | }
|
---|
716 |
|
---|
717 |
|
---|
718 | NX_INLINE NxQuat& NxQuat::operator-= (const NxQuat& q)
|
---|
719 | {
|
---|
720 | x-=q.x;
|
---|
721 | y-=q.y;
|
---|
722 | z-=q.z;
|
---|
723 | w-=q.w;
|
---|
724 | return *this;
|
---|
725 | }
|
---|
726 |
|
---|
727 |
|
---|
728 | NX_INLINE NxQuat& NxQuat::operator*= (const NxReal s)
|
---|
729 | {
|
---|
730 | x*=s;
|
---|
731 | y*=s;
|
---|
732 | z*=s;
|
---|
733 | w*=s;
|
---|
734 | return *this;
|
---|
735 | }
|
---|
736 |
|
---|
737 | NX_INLINE NxQuat::NxQuat(NxReal ix, NxReal iy, NxReal iz, NxReal iw)
|
---|
738 | {
|
---|
739 | x = ix;
|
---|
740 | y = iy;
|
---|
741 | z = iz;
|
---|
742 | w = iw;
|
---|
743 | }
|
---|
744 |
|
---|
745 | NX_INLINE NxQuat NxQuat::operator*(const NxQuat &q) const
|
---|
746 | {
|
---|
747 | return NxQuat(w*q.x + q.w*x + y*q.z - q.y*z,
|
---|
748 | w*q.y + q.w*y + z*q.x - q.z*x,
|
---|
749 | w*q.z + q.w*z + x*q.y - q.x*y,
|
---|
750 | w*q.w - x*q.x - y*q.y - z*q.z);
|
---|
751 | }
|
---|
752 |
|
---|
753 | NX_INLINE NxQuat NxQuat::operator+(const NxQuat &q) const
|
---|
754 | {
|
---|
755 | return NxQuat(x+q.x,y+q.y,z+q.z,w+q.w);
|
---|
756 | }
|
---|
757 |
|
---|
758 | NX_INLINE NxQuat NxQuat::operator-(const NxQuat &q) const
|
---|
759 | {
|
---|
760 | return NxQuat(x-q.x,y-q.y,z-q.z,w-q.w);
|
---|
761 | }
|
---|
762 |
|
---|
763 | NX_INLINE NxQuat NxQuat::operator!() const
|
---|
764 | {
|
---|
765 | return NxQuat(-x,-y,-z,w);
|
---|
766 | }
|
---|
767 |
|
---|
768 |
|
---|
769 |
|
---|
770 | NX_INLINE const NxVec3 NxQuat::rot(const NxVec3 &v) const
|
---|
771 | {
|
---|
772 | NxVec3 qv(x,y,z);
|
---|
773 |
|
---|
774 | return (v*(w*w-0.5f) + (qv^v)*w + qv*(qv|v))*2;
|
---|
775 | }
|
---|
776 |
|
---|
777 | NX_INLINE const NxVec3 NxQuat::invRot(const NxVec3 &v) const
|
---|
778 | {
|
---|
779 | NxVec3 qv(x,y,z);
|
---|
780 |
|
---|
781 | return (v*(w*w-0.5f) - (qv^v)*w + qv*(qv|v))*2;
|
---|
782 | }
|
---|
783 |
|
---|
784 |
|
---|
785 |
|
---|
786 | NX_INLINE const NxVec3 NxQuat::transform(const NxVec3 &v, const NxVec3 &p) const
|
---|
787 | {
|
---|
788 | return rot(v)+p;
|
---|
789 | }
|
---|
790 |
|
---|
791 | NX_INLINE const NxVec3 NxQuat::invTransform(const NxVec3 &v, const NxVec3 &p) const
|
---|
792 | {
|
---|
793 | return invRot(v-p);
|
---|
794 | }
|
---|
795 |
|
---|
796 | /** @} */
|
---|
797 | #endif
|
---|
798 |
|
---|
799 |
|
---|
800 | //AGCOPYRIGHTBEGIN
|
---|
801 | ///////////////////////////////////////////////////////////////////////////
|
---|
802 | // Copyright © 2005 AGEIA Technologies.
|
---|
803 | // All rights reserved. www.ageia.com
|
---|
804 | ///////////////////////////////////////////////////////////////////////////
|
---|
805 | //AGCOPYRIGHTEND
|
---|
806 |
|
---|