PxQuat.h

Go to the documentation of this file.
00001 //
00002 // Redistribution and use in source and binary forms, with or without
00003 // modification, are permitted provided that the following conditions
00004 // are met:
00005 //  * Redistributions of source code must retain the above copyright
00006 //    notice, this list of conditions and the following disclaimer.
00007 //  * Redistributions in binary form must reproduce the above copyright
00008 //    notice, this list of conditions and the following disclaimer in the
00009 //    documentation and/or other materials provided with the distribution.
00010 //  * Neither the name of NVIDIA CORPORATION nor the names of its
00011 //    contributors may be used to endorse or promote products derived
00012 //    from this software without specific prior written permission.
00013 //
00014 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
00015 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00016 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
00017 // PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
00018 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00019 // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00020 // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00021 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
00022 // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00023 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00024 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00025 //
00026 // Copyright (c) 2008-2018 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 #ifndef PXFOUNDATION_PXQUAT_H
00031 #define PXFOUNDATION_PXQUAT_H
00032 
00037 #include "foundation/PxVec3.h"
00038 #if !PX_DOXYGEN
00039 namespace physx
00040 {
00041 #endif
00042 
00049 class PxQuat
00050 {
00051   public:
00055     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat()
00056     {
00057     }
00058 
00060     PX_CUDA_CALLABLE PX_INLINE PxQuat(PxIDENTITY r) : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
00061     {
00062         PX_UNUSED(r);
00063     }
00064 
00068     explicit PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(float r) : x(0.0f), y(0.0f), z(0.0f), w(r)
00069     {
00070     }
00071 
00075     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(float nx, float ny, float nz, float nw) : x(nx), y(ny), z(nz), w(nw)
00076     {
00077     }
00078 
00088     PX_CUDA_CALLABLE PX_INLINE PxQuat(float angleRadians, const PxVec3& unitAxis)
00089     {
00090         PX_ASSERT(PxAbs(1.0f - unitAxis.magnitude()) < 1e-3f);
00091         const float a = angleRadians * 0.5f;
00092         const float s = PxSin(a);
00093         w = PxCos(a);
00094         x = unitAxis.x * s;
00095         y = unitAxis.y * s;
00096         z = unitAxis.z * s;
00097     }
00098 
00102     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat(const PxQuat& v) : x(v.x), y(v.y), z(v.z), w(v.w)
00103     {
00104     }
00105 
00111     PX_CUDA_CALLABLE PX_INLINE explicit PxQuat(const PxMat33& m); /* defined in PxMat33.h */
00112 
00116     PX_CUDA_CALLABLE PX_FORCE_INLINE bool isIdentity() const
00117     {
00118         return x==0.0f && y==0.0f && z==0.0f && w==1.0f;
00119     }
00120 
00124     PX_CUDA_CALLABLE bool isFinite() const
00125     {
00126         return PxIsFinite(x) && PxIsFinite(y) && PxIsFinite(z) && PxIsFinite(w);
00127     }
00128 
00132     PX_CUDA_CALLABLE bool isUnit() const
00133     {
00134         const float unitTolerance = 1e-4f;
00135         return isFinite() && PxAbs(magnitude() - 1) < unitTolerance;
00136     }
00137 
00142     PX_CUDA_CALLABLE bool isSane() const
00143     {
00144         const float unitTolerance = 1e-2f;
00145         return isFinite() && PxAbs(magnitude() - 1) < unitTolerance;
00146     }
00147 
00151     PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxQuat& q) const
00152     {
00153         return x == q.x && y == q.y && z == q.z && w == q.w;
00154     }
00155 
00159     PX_CUDA_CALLABLE PX_INLINE void toRadiansAndUnitAxis(float& angle, PxVec3& axis) const
00160     {
00161         const float quatEpsilon = 1.0e-8f;
00162         const float s2 = x * x + y * y + z * z;
00163         if(s2 < quatEpsilon * quatEpsilon) // can't extract a sensible axis
00164         {
00165             angle = 0.0f;
00166             axis = PxVec3(1.0f, 0.0f, 0.0f);
00167         }
00168         else
00169         {
00170             const float s = PxRecipSqrt(s2);
00171             axis = PxVec3(x, y, z) * s;
00172             angle = PxAbs(w) < quatEpsilon ? PxPi : PxAtan2(s2 * s, w) * 2.0f;
00173         }
00174     }
00175 
00181     PX_CUDA_CALLABLE PX_INLINE float getAngle() const
00182     {
00183         return PxAcos(w) * 2.0f;
00184     }
00185 
00191     PX_CUDA_CALLABLE PX_INLINE float getAngle(const PxQuat& q) const
00192     {
00193         return PxAcos(dot(q)) * 2.0f;
00194     }
00195 
00199     PX_CUDA_CALLABLE PX_FORCE_INLINE float magnitudeSquared() const
00200     {
00201         return x * x + y * y + z * z + w * w;
00202     }
00203 
00207     PX_CUDA_CALLABLE PX_FORCE_INLINE float dot(const PxQuat& v) const
00208     {
00209         return x * v.x + y * v.y + z * v.z + w * v.w;
00210     }
00211 
00212     PX_CUDA_CALLABLE PX_INLINE PxQuat getNormalized() const
00213     {
00214         const float s = 1.0f / magnitude();
00215         return PxQuat(x * s, y * s, z * s, w * s);
00216     }
00217 
00218     PX_CUDA_CALLABLE PX_INLINE float magnitude() const
00219     {
00220         return PxSqrt(magnitudeSquared());
00221     }
00222 
00223     // modifiers:
00227     PX_CUDA_CALLABLE PX_INLINE float normalize() // convert this PxQuat to a unit quaternion
00228     {
00229         const float mag = magnitude();
00230         if(mag != 0.0f)
00231         {
00232             const float imag = 1.0f / mag;
00233 
00234             x *= imag;
00235             y *= imag;
00236             z *= imag;
00237             w *= imag;
00238         }
00239         return mag;
00240     }
00241 
00242     /*
00243     \brief returns the conjugate.
00244 
00245     \note for unit quaternions, this is the inverse.
00246     */
00247     PX_CUDA_CALLABLE PX_INLINE PxQuat getConjugate() const
00248     {
00249         return PxQuat(-x, -y, -z, w);
00250     }
00251 
00252     /*
00253     \brief returns imaginary part.
00254     */
00255     PX_CUDA_CALLABLE PX_INLINE PxVec3 getImaginaryPart() const
00256     {
00257         return PxVec3(x, y, z);
00258     }
00259 
00261     PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector0() const
00262     {
00263         const float x2 = x * 2.0f;
00264         const float w2 = w * 2.0f;
00265         return PxVec3((w * w2) - 1.0f + x * x2, (z * w2) + y * x2, (-y * w2) + z * x2);
00266     }
00267 
00269     PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector1() const
00270     {
00271         const float y2 = y * 2.0f;
00272         const float w2 = w * 2.0f;
00273         return PxVec3((-z * w2) + x * y2, (w * w2) - 1.0f + y * y2, (x * w2) + z * y2);
00274     }
00275 
00277     PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 getBasisVector2() const
00278     {
00279         const float z2 = z * 2.0f;
00280         const float w2 = w * 2.0f;
00281         return PxVec3((y * w2) + x * z2, (-x * w2) + y * z2, (w * w2) - 1.0f + z * z2);
00282     }
00283 
00287     PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotate(const PxVec3& v) const
00288     {
00289         const float vx = 2.0f * v.x;
00290         const float vy = 2.0f * v.y;
00291         const float vz = 2.0f * v.z;
00292         const float w2 = w * w - 0.5f;
00293         const float dot2 = (x * vx + y * vy + z * vz);
00294         return PxVec3((vx * w2 + (y * vz - z * vy) * w + x * dot2), (vy * w2 + (z * vx - x * vz) * w + y * dot2),
00295                       (vz * w2 + (x * vy - y * vx) * w + z * dot2));
00296     }
00297 
00301     PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 rotateInv(const PxVec3& v) const
00302     {
00303         const float vx = 2.0f * v.x;
00304         const float vy = 2.0f * v.y;
00305         const float vz = 2.0f * v.z;
00306         const float w2 = w * w - 0.5f;
00307         const float dot2 = (x * vx + y * vy + z * vz);
00308         return PxVec3((vx * w2 - (y * vz - z * vy) * w + x * dot2), (vy * w2 - (z * vx - x * vz) * w + y * dot2),
00309                       (vz * w2 - (x * vy - y * vx) * w + z * dot2));
00310     }
00311 
00315     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator=(const PxQuat& p)
00316     {
00317         x = p.x;
00318         y = p.y;
00319         z = p.z;
00320         w = p.w;
00321         return *this;
00322     }
00323 
00324     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*=(const PxQuat& q)
00325     {
00326         const float tx = w * q.x + q.w * x + y * q.z - q.y * z;
00327         const float ty = w * q.y + q.w * y + z * q.x - q.z * x;
00328         const float tz = w * q.z + q.w * z + x * q.y - q.x * y;
00329 
00330         w = w * q.w - q.x * x - y * q.y - q.z * z;
00331         x = tx;
00332         y = ty;
00333         z = tz;
00334 
00335         return *this;
00336     }
00337 
00338     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator+=(const PxQuat& q)
00339     {
00340         x += q.x;
00341         y += q.y;
00342         z += q.z;
00343         w += q.w;
00344         return *this;
00345     }
00346 
00347     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator-=(const PxQuat& q)
00348     {
00349         x -= q.x;
00350         y -= q.y;
00351         z -= q.z;
00352         w -= q.w;
00353         return *this;
00354     }
00355 
00356     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat& operator*=(const float s)
00357     {
00358         x *= s;
00359         y *= s;
00360         z *= s;
00361         w *= s;
00362         return *this;
00363     }
00364 
00366     PX_CUDA_CALLABLE PX_INLINE PxQuat operator*(const PxQuat& q) const
00367     {
00368         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,
00369                       w * q.z + q.w * z + x * q.y - q.x * y, 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     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator-(const PxQuat& q) const
00385     {
00386         return PxQuat(x - q.x, y - q.y, z - q.z, w - q.w);
00387     }
00388 
00389     PX_CUDA_CALLABLE PX_FORCE_INLINE PxQuat operator*(float r) const
00390     {
00391         return PxQuat(x * r, y * r, z * r, w * r);
00392     }
00393 
00395     float x, y, z, w;
00396 };
00397 
00398 #if !PX_DOXYGEN
00399 } // namespace physx
00400 #endif
00401 
00403 #endif // #ifndef PXFOUNDATION_PXQUAT_H


Copyright © 2008-2018 NVIDIA Corporation, 2701 San Tomas Expressway, Santa Clara, CA 95050 U.S.A. All rights reserved. www.nvidia.com