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