1/*
2 * Copyright (c) 2008-2015, NVIDIA CORPORATION. All rights reserved.
3 *
4 * NVIDIA CORPORATION and its licensors retain all intellectual property
5 * and proprietary rights in and to this software, related documentation
6 * and any modifications thereto. Any use, reproduction, disclosure or
7 * distribution of this software and related documentation without an express
8 * license agreement from NVIDIA CORPORATION is strictly prohibited.
9 */
10// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
11// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
12
13
14
15#ifndef PX_FOUNDATION_PX_QUAT_H
16#define PX_FOUNDATION_PX_QUAT_H
17
18/** \addtogroup foundation
19@{
20*/
21
22#include "foundation/PxVec3.h"
23#ifndef PX_DOXYGEN
24namespace physx
25{
26#endif
27
28/**
29\brief This is a quaternion class. For more information on quaternion mathematics
30consult a mathematics source on complex numbers.
31
32*/
33
34class PxQuat
35{
36public:
37 /**
38 \brief Default constructor, does not do any initialization.
39 */
40 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat() { }
41
42
43 //! identity constructor
44 PX_CUDA_CALLABLE PX_INLINE PxQuat(PxIDENTITY r)
45 : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
46 {
47 PX_UNUSED(r);
48 }
49
50 /**
51 \brief Constructor from a scalar: sets the real part w to the scalar value, and the imaginary parts (x,y,z) to zero
52 */
53 explicit PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(PxReal r)
54 : x(0.0f), y(0.0f), z(0.0f), w(r)
55 {
56 }
57
58 /**
59 \brief Constructor. Take note of the order of the elements!
60 */
61 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(PxReal nx, PxReal ny, PxReal nz, PxReal nw) : x(nx),y(ny),z(nz),w(nw) {}
62
63 /**
64 \brief Creates from angle-axis representation.
65
66 Axis must be normalized!
67
68 Angle is in radians!
69
70 <b>Unit:</b> Radians
71 */
72 PX_CUDA_CALLABLE PX_INLINE PxQuat(PxReal angleRadians, const PxVec3& unitAxis)
73 {
74 PX_ASSERT(PxAbs(1.0f-unitAxis.magnitude())<1e-3f);
75 const PxReal a = angleRadians * 0.5f;
76 const PxReal s = PxSin(a);
77 w = PxCos(a);
78 x = unitAxis.x * s;
79 y = unitAxis.y * s;
80 z = unitAxis.z * s;
81 }
82
83 /**
84 \brief Copy ctor.
85 */
86 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(const PxQuat& v): x(v.x), y(v.y), z(v.z), w(v.w) {}
87
88 /**
89 \brief Creates from orientation matrix.
90
91 \param[in] m Rotation matrix to extract quaternion from.
92 */
93 PX_CUDA_CALLABLE PX_INLINE explicit PxQuat(const PxMat33& m); /* defined in PxMat33.h */
94
95 /**
96 \brief returns true if all elements are finite (not NAN or INF, etc.)
97 */
98 PX_CUDA_CALLABLE bool isFinite() const
99 {
100 return PxIsFinite(x)
101 && PxIsFinite(y)
102 && PxIsFinite(z)
103 && PxIsFinite(w);
104 }
105
106
107 /**
108 \brief returns true if finite and magnitude is close to unit
109 */
110
111 PX_CUDA_CALLABLE bool isUnit() const
112 {
113 const PxReal unitTolerance = 1e-4f;
114 return isFinite() && PxAbs(magnitude()-1)<unitTolerance;
115 }
116
117
118 /**
119 \brief returns true if finite and magnitude is reasonably close to unit to allow for some accumulation of error vs isValid
120 */
121
122 PX_CUDA_CALLABLE bool isSane() const
123 {
124 const PxReal unitTolerance = 1e-2f;
125 return isFinite() && PxAbs(magnitude()-1)<unitTolerance;
126 }
127
128 /**
129 \brief returns true if the two quaternions are exactly equal
130 */
131 PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxQuat&q) const { return x == q.x && y == q.y && z == q.z && w == q.w; }
132
133
134 /**
135 \brief converts this quaternion to angle-axis representation
136 */
137
138 PX_CUDA_CALLABLE PX_INLINE void toRadiansAndUnitAxis(PxReal& angle, PxVec3& axis) const
139 {
140 const PxReal quatEpsilon = 1.0e-8f;
141 const PxReal s2 = x*x+y*y+z*z;
142 if(s2<quatEpsilon*quatEpsilon) // can't extract a sensible axis
143 {
144 angle = 0.0f;
145 axis = PxVec3(1.0f,0.0f,0.0f);
146 }
147 else
148 {
149 const PxReal s = PxRecipSqrt(s2);
150 axis = PxVec3(x,y,z) * s;
151 angle = PxAbs(w)<quatEpsilon ? PxPi : PxAtan2(s2*s, w) * 2.0f;
152 }
153
154 }
155
156 /**
157 \brief Gets the angle between this quat and the identity quaternion.
158
159 <b>Unit:</b> Radians
160 */
161 PX_CUDA_CALLABLE PX_INLINE PxReal getAngle() const
162 {
163 return PxAcos(w) * 2.0f;
164 }
165
166
167 /**
168 \brief Gets the angle between this quat and the argument
169
170 <b>Unit:</b> Radians
171 */
172 PX_CUDA_CALLABLE PX_INLINE PxReal getAngle(const PxQuat& q) const
173 {
174 return PxAcos(dot(q)) * 2.0f;
175 }
176
177
178 /**
179 \brief This is the squared 4D vector length, should be 1 for unit quaternions.
180 */
181 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal magnitudeSquared() const
182 {
183 return x*x + y*y + z*z + w*w;
184 }
185
186 /**
187 \brief returns the scalar product of this and other.
188 */
189 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal dot(const PxQuat& v) const
190 {
191 return x * v.x + y * v.y + z * v.z + w * v.w;
192 }
193
194 PX_CUDA_CALLABLE PX_INLINE PxQuat getNormalized() const
195 {
196 const PxReal s = 1.0f/magnitude();
197 return PxQuat(x*s, y*s, z*s, w*s);
198 }
199
200
201 PX_CUDA_CALLABLE PX_INLINE float magnitude() const
202 {
203 return PxSqrt(magnitudeSquared());
204 }
205
206 //modifiers:
207 /**
208 \brief maps to the closest unit quaternion.
209 */
210 PX_CUDA_CALLABLE PX_INLINE PxReal normalize() // convert this PxQuat to a unit quaternion
211 {
212 const PxReal mag = magnitude();
213 if (mag)
214 {
215 const PxReal imag = 1.0f / mag;
216
217 x *= imag;
218 y *= imag;
219 z *= imag;
220 w *= imag;
221 }
222 return mag;
223 }
224
225 /*
226 \brief returns the conjugate.
227
228 \note for unit quaternions, this is the inverse.
229 */
230 PX_CUDA_CALLABLE PX_INLINE PxQuat getConjugate() const
231 {
232 return PxQuat(-x,-y,-z,w);
233 }
234
235 /*
236 \brief returns imaginary part.
237 */
238 PX_CUDA_CALLABLE PX_INLINE PxVec3 getImaginaryPart() const
239 {
240 return PxVec3(x,y,z);
241 }
242
243 /** brief computes rotation of x-axis */
244 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector0() const
245 {
246 const PxF32 x2 = x*2.0f;
247 const PxF32 w2 = w*2.0f;
248 return PxVec3( (w * w2) - 1.0f + x*x2,
249 (z * w2) + y*x2,
250 (-y * w2) + z*x2);
251 }
252
253 /** brief computes rotation of y-axis */
254 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector1() const
255 {
256 const PxF32 y2 = y*2.0f;
257 const PxF32 w2 = w*2.0f;
258 return PxVec3( (-z * w2) + x*y2,
259 (w * w2) - 1.0f + y*y2,
260 (x * w2) + z*y2);
261 }
262
263
264 /** brief computes rotation of z-axis */
265 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector2() const
266 {
267 const PxF32 z2 = z*2.0f;
268 const PxF32 w2 = w*2.0f;
269 return PxVec3( (y * w2) + x*z2,
270 (-x * w2) + y*z2,
271 (w * w2) - 1.0f + z*z2);
272 }
273
274 /**
275 rotates passed vec by this (assumed unitary)
276 */
277 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotate(const PxVec3& v) const
278 {
279 const PxF32 vx = 2.0f*v.x;
280 const PxF32 vy = 2.0f*v.y;
281 const PxF32 vz = 2.0f*v.z;
282 const PxF32 w2 = w*w-0.5f;
283 const PxF32 dot2 = (x*vx + y*vy +z*vz);
284 return PxVec3
285 (
286 (vx*w2 + (y * vz - z * vy)*w + x*dot2),
287 (vy*w2 + (z * vx - x * vz)*w + y*dot2),
288 (vz*w2 + (x * vy - y * vx)*w + z*dot2)
289 );
290 }
291
292 /**
293 inverse rotates passed vec by this (assumed unitary)
294 */
295 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotateInv(const PxVec3& v) const
296 {
297 const PxF32 vx = 2.0f*v.x;
298 const PxF32 vy = 2.0f*v.y;
299 const PxF32 vz = 2.0f*v.z;
300 const PxF32 w2 = w*w-0.5f;
301 const PxF32 dot2 = (x*vx + y*vy +z*vz);
302 return PxVec3
303 (
304 (vx*w2 - (y * vz - z * vy)*w + x*dot2),
305 (vy*w2 - (z * vx - x * vz)*w + y*dot2),
306 (vz*w2 - (x * vy - y * vx)*w + z*dot2)
307 );
308 }
309
310 /**
311 \brief Assignment operator
312 */
313 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator=(const PxQuat& p) { x = p.x; y = p.y; z = p.z; w = p.w; return *this; }
314
315 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*= (const PxQuat& q)
316 {
317 const PxReal tx = w*q.x + q.w*x + y*q.z - q.y*z;
318 const PxReal ty = w*q.y + q.w*y + z*q.x - q.z*x;
319 const PxReal tz = w*q.z + q.w*z + x*q.y - q.x*y;
320
321 w = w*q.w - q.x*x - y*q.y - q.z*z;
322 x = tx;
323 y = ty;
324 z = tz;
325
326 return *this;
327 }
328
329 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator+= (const PxQuat& q)
330 {
331 x+=q.x;
332 y+=q.y;
333 z+=q.z;
334 w+=q.w;
335 return *this;
336 }
337
338 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator-= (const PxQuat& q)
339 {
340 x-=q.x;
341 y-=q.y;
342 z-=q.z;
343 w-=q.w;
344 return *this;
345 }
346
347 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*= (const PxReal s)
348 {
349 x*=s;
350 y*=s;
351 z*=s;
352 w*=s;
353 return *this;
354 }
355
356 /** quaternion multiplication */
357 PX_CUDA_CALLABLE PX_INLINE PxQuat operator*(const PxQuat& q) const
358 {
359 return PxQuat(w*q.x + q.w*x + y*q.z - q.y*z,
360 w*q.y + q.w*y + z*q.x - q.z*x,
361 w*q.z + q.w*z + x*q.y - q.x*y,
362 w*q.w - x*q.x - y*q.y - z*q.z);
363 }
364
365 /** quaternion addition */
366 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator+(const PxQuat& q) const
367 {
368 return PxQuat(x+q.x,y+q.y,z+q.z,w+q.w);
369 }
370
371 /** quaternion subtraction */
372 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator-() const
373 {
374 return PxQuat(-x,-y,-z,-w);
375 }
376
377
378 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator-(const PxQuat& q) const
379 {
380 return PxQuat(x-q.x,y-q.y,z-q.z,w-q.w);
381 }
382
383
384 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator*(PxReal r) const
385 {
386 return PxQuat(x*r,y*r,z*r,w*r);
387 }
388
389
390 /** \deprecated use PxQuat(PxIdentity) */
391 PX_DEPRECATED static PX_CUDA_CALLABLE PX_INLINE PxQuat createIdentity() { return PxQuat(PxIdentity); }
392
393 /** the quaternion elements */
394 PxReal x,y,z,w;
395};
396
397#ifndef PX_DOXYGEN
398} // namespace physx
399#endif
400
401/** @} */
402#endif // PX_FOUNDATION_PX_QUAT_H
403