PxQuat.h
Go to the documentation of this file.00001 // This code contains NVIDIA Confidential Information and is disclosed to you 00002 // under a form of NVIDIA software license agreement provided separately to you. 00003 // 00004 // Notice 00005 // NVIDIA Corporation and its licensors retain all intellectual property and 00006 // proprietary rights in and to this software and related documentation and 00007 // any modifications thereto. Any use, reproduction, disclosure, or 00008 // distribution of this software and related documentation without an express 00009 // license agreement from NVIDIA Corporation is strictly prohibited. 00010 // 00011 // ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES 00012 // NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO 00013 // THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, 00014 // MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. 00015 // 00016 // Information and code furnished is believed to be accurate and reliable. 00017 // However, NVIDIA Corporation assumes no responsibility for the consequences of use of such 00018 // information or for any infringement of patents or other rights of third parties that may 00019 // result from its use. No license is granted by implication or otherwise under any patent 00020 // or patent rights of NVIDIA Corporation. Details are subject to change without notice. 00021 // This code supersedes and replaces all information previously supplied. 00022 // NVIDIA Corporation products are not authorized for use as critical 00023 // components in life support devices or systems without express written approval of 00024 // NVIDIA Corporation. 00025 // 00026 // Copyright (c) 2008-2012 NVIDIA Corporation. All rights reserved. 00027 // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. 00028 // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. 00029 00030 00031 00032 #ifndef PX_FOUNDATION_PX_QUAT_H 00033 #define PX_FOUNDATION_PX_QUAT_H 00034 00039 #include "foundation/PxVec3.h" 00040 #ifndef PX_DOXYGEN 00041 namespace physx 00042 { 00043 #endif 00044 00051 class PxQuat 00052 { 00053 public: 00057 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat() { } 00058 00059 00063 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(PxReal nx, PxReal ny, PxReal nz, PxReal nw) : x(nx),y(ny),z(nz),w(nw) {} 00064 00074 PX_CUDA_CALLABLE PX_INLINE PxQuat(PxReal angleRadians, const PxVec3& unitAxis) 00075 { 00076 PX_ASSERT(PxAbs(1.0f-unitAxis.magnitude())<1e-3f); 00077 const PxReal a = angleRadians * 0.5f; 00078 const PxReal s = PxSin(a); 00079 w = PxCos(a); 00080 x = unitAxis.x * s; 00081 y = unitAxis.y * s; 00082 z = unitAxis.z * s; 00083 } 00084 00088 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(const PxQuat& v): x(v.x), y(v.y), z(v.z), w(v.w) {} 00089 00095 PX_CUDA_CALLABLE PX_INLINE explicit PxQuat(const PxMat33& m); /* defined in PxMat33.h */ 00096 00100 PX_CUDA_CALLABLE bool isFinite() const 00101 { 00102 return PxIsFinite(x) 00103 && PxIsFinite(y) 00104 && PxIsFinite(z) 00105 && PxIsFinite(w); 00106 } 00107 00108 00113 PX_CUDA_CALLABLE bool isUnit() const 00114 { 00115 const PxReal unitTolerance = PxReal(1e-4); 00116 return isFinite() && PxAbs(magnitude()-1)<unitTolerance; 00117 } 00118 00119 00124 PX_CUDA_CALLABLE bool isSane() const 00125 { 00126 const PxReal unitTolerance = PxReal(1e-2); 00127 return isFinite() && PxAbs(magnitude()-1)<unitTolerance; 00128 } 00129 00134 PX_CUDA_CALLABLE PX_INLINE void toRadiansAndUnitAxis(PxReal& angle, PxVec3& axis) const 00135 { 00136 const PxReal quatEpsilon = (PxReal(1.0e-8f)); 00137 const PxReal s2 = x*x+y*y+z*z; 00138 if(s2<quatEpsilon*quatEpsilon) // can't extract a sensible axis 00139 { 00140 angle = 0; 00141 axis = PxVec3(1,0,0); 00142 } 00143 else 00144 { 00145 const PxReal s = PxRecipSqrt(s2); 00146 axis = PxVec3(x,y,z) * s; 00147 angle = w<quatEpsilon ? PxPi : PxAtan2(s2*s, w) * 2; 00148 } 00149 00150 } 00151 00157 PX_CUDA_CALLABLE PX_INLINE PxReal getAngle() const 00158 { 00159 return PxAcos(w) * PxReal(2); 00160 } 00161 00162 00168 PX_CUDA_CALLABLE PX_INLINE PxReal getAngle(const PxQuat& q) const 00169 { 00170 return PxAcos(dot(q)) * PxReal(2); 00171 } 00172 00173 00177 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal magnitudeSquared() const 00178 { 00179 return x*x + y*y + z*z + w*w; 00180 } 00181 00185 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal dot(const PxQuat& v) const 00186 { 00187 return x * v.x + y * v.y + z * v.z + w * v.w; 00188 } 00189 00190 PX_CUDA_CALLABLE PX_INLINE PxQuat getNormalized() const 00191 { 00192 const PxReal s = 1.0f/magnitude(); 00193 return PxQuat(x*s, y*s, z*s, w*s); 00194 } 00195 00196 00197 PX_CUDA_CALLABLE PX_INLINE float magnitude() const 00198 { 00199 return PxSqrt(magnitudeSquared()); 00200 } 00201 00202 //modifiers: 00206 PX_CUDA_CALLABLE PX_INLINE PxReal normalize() // convert this PxQuat to a unit quaternion 00207 { 00208 const PxReal mag = magnitude(); 00209 if (mag) 00210 { 00211 const PxReal imag = PxReal(1) / mag; 00212 00213 x *= imag; 00214 y *= imag; 00215 z *= imag; 00216 w *= imag; 00217 } 00218 return mag; 00219 } 00220 00221 /* 00222 \brief returns the conjugate. 00223 00224 \note for unit quaternions, this is the inverse. 00225 */ 00226 PX_CUDA_CALLABLE PX_INLINE PxQuat getConjugate() const 00227 { 00228 return PxQuat(-x,-y,-z,w); 00229 } 00230 00231 /* 00232 \brief returns imaginary part. 00233 */ 00234 PX_CUDA_CALLABLE PX_INLINE PxVec3 getImaginaryPart() const 00235 { 00236 return PxVec3(x,y,z); 00237 } 00238 00240 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector0() const 00241 { 00242 // return rotate(PxVec3(1,0,0)); 00243 const PxF32 x2 = x*2.0f; 00244 const PxF32 w2 = w*2.0f; 00245 return PxVec3( (w * w2) - 1.0f + x*x2, 00246 (z * w2) + y*x2, 00247 (-y * w2) + z*x2); 00248 } 00249 00251 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector1() const 00252 { 00253 // return rotate(PxVec3(0,1,0)); 00254 const PxF32 y2 = y*2.0f; 00255 const PxF32 w2 = w*2.0f; 00256 return PxVec3( (-z * w2) + x*y2, 00257 (w * w2) - 1.0f + y*y2, 00258 (x * w2) + z*y2); 00259 } 00260 00261 00263 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector2() const 00264 { 00265 // return rotate(PxVec3(0,0,1)); 00266 const PxF32 z2 = z*2.0f; 00267 const PxF32 w2 = w*2.0f; 00268 return PxVec3( (y * w2) + x*z2, 00269 (-x * w2) + y*z2, 00270 (w * w2) - 1.0f + z*z2); 00271 } 00272 00276 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotate(const PxVec3& v) const 00277 // PX_CUDA_CALLABLE PX_INLINE const PxVec3 rotate(const PxVec3& v) const 00278 { 00279 const PxF32 vx = 2.0f*v.x; 00280 const PxF32 vy = 2.0f*v.y; 00281 const PxF32 vz = 2.0f*v.z; 00282 const PxF32 w2 = w*w-0.5f; 00283 const PxF32 dot2 = (x*vx + y*vy +z*vz); 00284 return PxVec3 00285 ( 00286 (vx*w2 + (y * vz - z * vy)*w + x*dot2), 00287 (vy*w2 + (z * vx - x * vz)*w + y*dot2), 00288 (vz*w2 + (x * vy - y * vx)*w + z*dot2) 00289 ); 00290 /* 00291 const PxVec3 qv(x,y,z); 00292 return (v*(w*w-0.5f) + (qv.cross(v))*w + qv*(qv.dot(v)))*2; 00293 */ 00294 } 00295 00299 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotateInv(const PxVec3& v) const 00300 // PX_CUDA_CALLABLE PX_INLINE const PxVec3 rotateInv(const PxVec3& v) const 00301 { 00302 const PxF32 vx = 2.0f*v.x; 00303 const PxF32 vy = 2.0f*v.y; 00304 const PxF32 vz = 2.0f*v.z; 00305 const PxF32 w2 = w*w-0.5f; 00306 const PxF32 dot2 = (x*vx + y*vy +z*vz); 00307 return PxVec3 00308 ( 00309 (vx*w2 - (y * vz - z * vy)*w + x*dot2), 00310 (vy*w2 - (z * vx - x * vz)*w + y*dot2), 00311 (vz*w2 - (x * vy - y * vx)*w + z*dot2) 00312 ); 00313 // const PxVec3 qv(x,y,z); 00314 // return (v*(w*w-0.5f) - (qv.cross(v))*w + qv*(qv.dot(v)))*2; 00315 } 00316 00320 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; } 00321 00322 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*= (const PxQuat& q) 00323 { 00324 const PxReal tx = w*q.x + q.w*x + y*q.z - q.y*z; 00325 const PxReal ty = w*q.y + q.w*y + z*q.x - q.z*x; 00326 const PxReal tz = w*q.z + q.w*z + x*q.y - q.x*y; 00327 00328 w = w*q.w - q.x*x - y*q.y - q.z*z; 00329 x = tx; 00330 y = ty; 00331 z = tz; 00332 00333 return *this; 00334 } 00335 00336 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator+= (const PxQuat& q) 00337 { 00338 x+=q.x; 00339 y+=q.y; 00340 z+=q.z; 00341 w+=q.w; 00342 return *this; 00343 } 00344 00345 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator-= (const PxQuat& q) 00346 { 00347 x-=q.x; 00348 y-=q.y; 00349 z-=q.z; 00350 w-=q.w; 00351 return *this; 00352 } 00353 00354 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*= (const PxReal s) 00355 { 00356 x*=s; 00357 y*=s; 00358 z*=s; 00359 w*=s; 00360 return *this; 00361 } 00362 00364 PX_CUDA_CALLABLE PX_INLINE PxQuat operator*(const PxQuat& q) const 00365 { 00366 return PxQuat(w*q.x + q.w*x + y*q.z - q.y*z, 00367 w*q.y + q.w*y + z*q.x - q.z*x, 00368 w*q.z + q.w*z + x*q.y - q.x*y, 00369 w*q.w - x*q.x - y*q.y - z*q.z); 00370 } 00371 00373 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator+(const PxQuat& q) const 00374 { 00375 return PxQuat(x+q.x,y+q.y,z+q.z,w+q.w); 00376 } 00377 00379 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator-() const 00380 { 00381 return PxQuat(-x,-y,-z,-w); 00382 } 00383 00384 00385 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator-(const PxQuat& q) const 00386 { 00387 return PxQuat(x-q.x,y-q.y,z-q.z,w-q.w); 00388 } 00389 00390 00391 PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator*(PxReal r) const 00392 { 00393 return PxQuat(x*r,y*r,z*r,w*r); 00394 } 00395 00396 static PX_CUDA_CALLABLE PX_INLINE PxQuat createIdentity() { return PxQuat(0,0,0,1); } 00397 00399 PxReal x,y,z,w; 00400 }; 00401 00402 #ifndef PX_DOXYGEN 00403 } // namespace physx 00404 #endif 00405 00407 #endif // PX_FOUNDATION_PX_QUAT_H
Copyright © 2008-2012 NVIDIA Corporation, 2701 San Tomas Expressway, Santa Clara, CA 95050 U.S.A. All rights reserved. www.nvidia.com