D:/SoftwareEntwicklung/C++/HLC/Tools/SoftPixelEngine/repository/sources/Base/spDimensionQuaternion.hpp
Go to the documentation of this file.
00001 /* 00002 * Quaternion header 00003 * 00004 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns) 00005 * See "SoftPixelEngine.hpp" for license information. 00006 */ 00007 00008 #ifndef __SP_DIMENSION_QUATERNION_H__ 00009 #define __SP_DIMENSION_QUATERNION_H__ 00010 00011 00012 #include "Base/spStandard.hpp" 00013 #include "Base/spDimensionVector3D.hpp" 00014 #include "Base/spDimensionMatrix4.hpp" 00015 00016 #include <math.h> 00017 00018 00019 namespace sp 00020 { 00021 namespace dim 00022 { 00023 00024 00025 template <typename T> class quaternion4 00026 { 00027 00028 public: 00029 00030 quaternion4() : 00031 X(0), 00032 Y(0), 00033 Z(0), 00034 W(1) 00035 { 00036 } 00037 quaternion4(const T &x, const T &y, const T &z) : 00038 X(0), 00039 Y(0), 00040 Z(0), 00041 W(1) 00042 { 00043 set(x, y, z); 00044 } 00045 quaternion4(const T &x, const T &y, const T &z, const T &w) : 00046 X(x), 00047 Y(y), 00048 Z(z), 00049 W(w) 00050 { 00051 } 00052 quaternion4(const vector3d<T> &Vec) : 00053 X(0), 00054 Y(0), 00055 Z(0), 00056 W(1) 00057 { 00058 set(Vec); 00059 } 00060 quaternion4(const vector4d<T> &Vec) : 00061 X(Vec.X), 00062 Y(Vec.Y), 00063 Z(Vec.Z), 00064 W(Vec.W) 00065 { 00066 } 00067 quaternion4(const quaternion4<T> &Other) : 00068 X(Other.X), 00069 Y(Other.Y), 00070 Z(Other.Z), 00071 W(Other.W) 00072 { 00073 } 00074 quaternion4(const matrix4<T> &Matrix) : 00075 X(0), 00076 Y(0), 00077 Z(0), 00078 W(1) 00079 { 00080 setMatrix(Matrix); 00081 } 00082 ~quaternion4() 00083 { 00084 } 00085 00086 /* === Operators - copying === */ 00087 00088 inline quaternion4<T>& operator = (const quaternion4<T> &Other) 00089 { 00090 set(Other.X, Other.Y, Other.Z, Other.W); 00091 return *this; 00092 } 00093 00094 /* === Operators - comparisions === */ 00095 00096 inline bool operator == (const quaternion4<T> &Other) const 00097 { 00098 return X == Other.X && Y == Other.Y && Z == Other.Z && W == Other.W; 00099 } 00100 inline bool operator != (const quaternion4<T> &Other) const 00101 { 00102 return X != Other.X || Y != Other.Y || Z != Other.Z || W != Other.W; 00103 } 00104 00105 inline bool operator < (const quaternion4<T> &Other) const 00106 { 00107 return (X == Other.X) ? ( (Y == Other.Y) ? ( (Z == Other.Z) ? W < Other.W : Z < Other.Z ) : Y < Other.Y ) : X < Other.X; 00108 } 00109 inline bool operator > (const quaternion4<T> &Other) const 00110 { 00111 return (X == Other.X) ? ( (Y == Other.Y) ? ( (Z == Other.Z) ? W > Other.W : Z > Other.Z ) : Y > Other.Y ) : X > Other.X; 00112 } 00113 00114 inline bool operator <= (const quaternion4<T> &Other) const 00115 { 00116 return (X == Other.X) ? ( (Y == Other.Y) ? ( (Z == Other.Z) ? W <= Other.W : Z <= Other.Z ) : Y <= Other.Y ) : X <= Other.X; 00117 } 00118 inline bool operator >= (const quaternion4<T> &Other) const 00119 { 00120 return (X == Other.X) ? ( (Y == Other.Y) ? ( (Z == Other.Z) ? W >= Other.W : Z >= Other.Z ) : Y >= Other.Y ) : X >= Other.X; 00121 } 00122 00123 /* === Operators - addition, subtraction, division, multiplication === */ 00124 00125 inline quaternion4<T> operator + (const quaternion4<T> &Other) const 00126 { 00127 return quaternion4<T>(X + Other.X, Y + Other.Y, Z + Other.Z, W + Other.W); 00128 } 00129 inline quaternion4<T>& operator += (const quaternion4<T> &Other) 00130 { 00131 X += Other.X; Y += Other.Y; Z += Other.Z; W += Other.W; return *this; 00132 } 00133 00134 inline quaternion4<T> operator - (const quaternion4<T> &Other) const 00135 { 00136 return quaternion4<T>(X - Other.X, Y - Other.Y, Z - Other.Z, W - Other.W); 00137 } 00138 inline quaternion4<T>& operator -= (const quaternion4<T> &Other) 00139 { 00140 X -= Other.X; Y -= Other.Y; Z -= Other.Z; W -= Other.W; return *this; 00141 } 00142 00143 inline quaternion4<T> operator / (const quaternion4<T> &Other) const 00144 { 00145 return quaternion4<T>(X / Other.X, Y / Other.Y, Z / Other.Z, W / Other.W); 00146 } 00147 inline quaternion4<T>& operator /= (const quaternion4<T> &Other) 00148 { 00149 X /= Other.X; Y /= Other.Y; Z /= Other.Z; W /= Other.W; return *this; 00150 } 00151 00152 inline quaternion4<T> operator * (const quaternion4<T> &Other) const 00153 { 00154 quaternion4<T> tmp; 00155 00156 tmp.W = (Other.W * W) - (Other.X * X) - (Other.Y * Y) - (Other.Z * Z); 00157 tmp.X = (Other.W * X) + (Other.X * W) + (Other.Y * Z) - (Other.Z * Y); 00158 tmp.Y = (Other.W * Y) + (Other.Y * W) + (Other.Z * X) - (Other.X * Z); 00159 tmp.Z = (Other.W * Z) + (Other.Z * W) + (Other.X * Y) - (Other.Y * X); 00160 00161 return tmp; 00162 } 00163 inline quaternion4<T>& operator *= (const quaternion4<T> &Other) 00164 { 00165 *this = *this * Other; return *this; 00166 } 00167 00168 inline vector3d<T> operator * (const vector3d<T> &Vector) const 00169 { 00170 vector3d<T> uv, uuv; 00171 vector3d<T> qvec(X, Y, Z); 00172 00173 uv = qvec.cross(Vector); 00174 uuv = qvec.cross(uv); 00175 uv *= (T(2) * W); 00176 uuv *= T(2); 00177 00178 /* Result := Vector + uv + uuv */ 00179 uv += uuv; 00180 uv += Vector; 00181 return uv; 00182 } 00183 00184 inline quaternion4 operator / (const T &Size) const 00185 { 00186 return quaternion4(X / Size, Y / Size, Z / Size, W / Size); 00187 } 00188 inline quaternion4& operator /= (const T &Size) 00189 { 00190 X /= Size; Y /= Size; Z /= Size; W /= Size; return *this; 00191 } 00192 00193 inline quaternion4 operator * (const T &Size) const 00194 { 00195 return quaternion4(X * Size, Y * Size, Z * Size, W * Size); 00196 } 00197 inline quaternion4& operator *= (const T &Size) 00198 { 00199 X *= Size; Y *= Size; Z *= Size; W *= Size; return *this; 00200 } 00201 00202 /* === Additional operators === */ 00203 00204 inline const T operator [] (u32 i) const 00205 { 00206 return i < 4 ? *(&X + i) : T(0); 00207 } 00208 00209 inline T& operator [] (u32 i) 00210 { 00211 return *(&X + i); 00212 } 00213 00214 /* === Extra functions === */ 00215 00216 inline T dot(const quaternion4<T> &Other) const 00217 { 00218 return X*Other.X + Y*Other.Y + Z*Other.Z + W*Other.W; 00219 } 00220 00221 inline quaternion4<T>& normalize() 00222 { 00223 T n = X*X + Y*Y + Z*Z + W*W; 00224 00225 if (n == T(1) || n == T(0)) 00226 return *this; 00227 00228 n = T(1) / sqrt(n); 00229 00230 X *= n; 00231 Y *= n; 00232 Z *= n; 00233 W *= n; 00234 00235 return *this; 00236 } 00237 00238 inline quaternion4& setInverse() 00239 { 00240 X = -X; Y = -Y; Z = -Z; return *this; 00241 } 00242 inline quaternion4 getInverse() const 00243 { 00244 return quaternion4(-X, -Y, -Z, W); 00245 } 00246 00247 inline void set(const T &NewX, const T &NewY, const T &NewZ, const T &NewW) 00248 { 00249 X = NewX; 00250 Y = NewY; 00251 Z = NewZ; 00252 W = NewW; 00253 } 00254 00255 inline void set(const T &NewX, const T &NewY, const T &NewZ) 00256 { 00257 const T cp = cos(NewX/2); 00258 const T cr = cos(NewZ/2); 00259 const T cy = cos(NewY/2); 00260 00261 const T sp = sin(NewX/2); 00262 const T sr = sin(NewZ/2); 00263 const T sy = sin(NewY/2); 00264 00265 const T cpcy = cp * cy; 00266 const T spsy = sp * sy; 00267 const T cpsy = cp * sy; 00268 const T spcy = sp * cy; 00269 00270 X = sr * cpcy - cr * spsy; 00271 Y = cr * spcy + sr * cpsy; 00272 Z = cr * cpsy - sr * spcy; 00273 W = cr * cpcy + sr * spsy; 00274 00275 normalize(); 00276 } 00277 00278 inline void set(const vector3d<T> &Vector) 00279 { 00280 set(Vector.X, Vector.Y, Vector.Z); 00281 } 00282 inline void set(const vector4d<T> &Vector) 00283 { 00284 set(Vector.X, Vector.Y, Vector.Z, Vector.W); 00285 } 00286 00287 inline void getMatrix(matrix4<T> &Mat) const 00288 { 00289 Mat[ 0] = 1.0f - T(2)*Y*Y - T(2)*Z*Z; 00290 Mat[ 1] = T(2)*X*Y + T(2)*Z*W; 00291 Mat[ 2] = T(2)*X*Z - T(2)*Y*W; 00292 Mat[ 3] = 0.0f; 00293 00294 Mat[ 4] = T(2)*X*Y - T(2)*Z*W; 00295 Mat[ 5] = 1.0f - T(2)*X*X - T(2)*Z*Z; 00296 Mat[ 6] = T(2)*Z*Y + T(2)*X*W; 00297 Mat[ 7] = 0.0f; 00298 00299 Mat[ 8] = T(2)*X*Z + T(2)*Y*W; 00300 Mat[ 9] = T(2)*Z*Y - T(2)*X*W; 00301 Mat[10] = 1.0f - T(2)*X*X - T(2)*Y*Y; 00302 Mat[11] = T(0); 00303 00304 Mat[12] = T(0); 00305 Mat[13] = T(0); 00306 Mat[14] = T(0); 00307 Mat[15] = 1.0f; 00308 } 00309 00310 inline matrix4<T> getMatrix() const 00311 { 00312 matrix4<T> Mat; 00313 getMatrix(Mat); 00314 return Mat; 00315 } 00316 00317 inline void getMatrixTransposed(matrix4<T> &Mat) const 00318 { 00319 Mat[ 0] = T(1) - T(2)*Y*Y - T(2)*Z*Z; 00320 Mat[ 4] = T(2)*X*Y + T(2)*Z*W; 00321 Mat[ 8] = T(2)*X*Z - T(2)*Y*W; 00322 Mat[12] = T(0); 00323 00324 Mat[ 1] = T(2)*X*Y - T(2)*Z*W; 00325 Mat[ 5] = T(1) - T(2)*X*X - T(2)*Z*Z; 00326 Mat[ 9] = T(2)*Z*Y + T(2)*X*W; 00327 Mat[13] = T(0); 00328 00329 Mat[ 2] = T(2)*X*Z + T(2)*Y*W; 00330 Mat[ 6] = T(2)*Z*Y - T(2)*X*W; 00331 Mat[10] = T(1) - T(2)*X*X - T(2)*Y*Y; 00332 Mat[14] = T(0); 00333 00334 Mat[ 3] = T(0); 00335 Mat[ 7] = T(0); 00336 Mat[11] = T(0); 00337 Mat[15] = T(1); 00338 } 00339 00340 inline matrix4<T> getMatrixTransposed() const 00341 { 00342 matrix4<T> Mat; 00343 getMatrixTransposed(Mat); 00344 return Mat; 00345 } 00346 00347 inline void setMatrix(const matrix4<T> &Mat) 00348 { 00349 T trace = Mat(0, 0) + Mat(1, 1) + Mat(2, 2) + 1.0f; 00350 00351 if (trace > T(0)) 00352 { 00353 const T s = T(2) * sqrt(trace); 00354 X = (Mat(2, 1) - Mat(1, 2)) / s; 00355 Y = (Mat(0, 2) - Mat(2, 0)) / s; 00356 Z = (Mat(1, 0) - Mat(0, 1)) / s; 00357 W = T(0.25) * s; 00358 } 00359 else 00360 { 00361 if (Mat(0, 0) > Mat(1, 1) && Mat(0, 0) > Mat(2, 2)) 00362 { 00363 const T s = T(2) * sqrtf(1.0f + Mat(0, 0) - Mat(1, 1) - Mat(2, 2)); 00364 X = T(0.25) * s; 00365 Y = (Mat(0, 1) + Mat(1, 0) ) / s; 00366 Z = (Mat(2, 0) + Mat(0, 2) ) / s; 00367 W = (Mat(2, 1) - Mat(1, 2) ) / s; 00368 } 00369 else if (Mat(1, 1) > Mat(2, 2)) 00370 { 00371 const T s = T(2) * sqrtf(1.0f + Mat(1, 1) - Mat(0, 0) - Mat(2, 2)); 00372 X = (Mat(0, 1) + Mat(1, 0) ) / s; 00373 Y = T(0.25) * s; 00374 Z = (Mat(1, 2) + Mat(2, 1) ) / s; 00375 W = (Mat(0, 2) - Mat(2, 0) ) / s; 00376 } 00377 else 00378 { 00379 const T s = T(2) * sqrtf(1.0f + Mat(2, 2) - Mat(0, 0) - Mat(1, 1)); 00380 X = (Mat(0, 2) + Mat(2, 0) ) / s; 00381 Y = (Mat(1, 2) + Mat(2, 1) ) / s; 00382 Z = T(0.25) * s; 00383 W = (Mat(1, 0) - Mat(0, 1) ) / s; 00384 } 00385 } 00386 00387 normalize(); 00388 } 00389 00390 inline quaternion4<T>& setAngleAxis(const T &Angle, const vector3d<T> &Axis) 00391 { 00392 const T HalfAngle = T(0.5) * Angle; 00393 const T Sine = sin(HalfAngle); 00394 00395 X = Sine * Axis.X; 00396 Y = Sine * Axis.Y; 00397 Z = Sine * Axis.Z; 00398 W = cos(HalfAngle); 00399 00400 return *this; 00401 } 00402 00403 inline void getAngleAxis(T &Angle, vector3d<T> &Axis) const 00404 { 00405 const T Scale = sqrt(X*X + Y*Y + Z*Z); 00406 00407 if ( ( Scale > T(-1.0e-6) && Scale < T(1.0e-6) ) || W > T(1) || W < T(-1) ) 00408 { 00409 Axis.X = T(0); 00410 Axis.Y = 1.0f; 00411 Axis.Z = T(0); 00412 Angle = T(0); 00413 } 00414 else 00415 { 00416 const T InvScale = T(1) / Scale; 00417 Axis.X = X * InvScale; 00418 Axis.Y = Y * InvScale; 00419 Axis.Z = Z * InvScale; 00420 Angle = T(2) * acos(W); 00421 } 00422 } 00423 00424 inline void getEuler(vector3d<T> &Euler) const 00425 { 00426 const T sqX = X*X; 00427 const T sqY = Y*Y; 00428 const T sqZ = Z*Z; 00429 const T sqW = W*W; 00430 00431 T tmp = T(-2) * (X*Z - Y*W); 00432 00433 math::Clamp(tmp, T(-1), T(1)); 00434 00435 Euler.X = atan2(T(2) * (Y*Z + X*W), -sqX - sqY + sqZ + sqW); 00436 Euler.Y = asin(tmp); 00437 Euler.Z = atan2(T(2) * (X*Y + Z*W), sqX - sqY - sqZ + sqW); 00438 } 00439 00440 /* 00441 * Slerp: "spherical linear interpolation" 00442 * Smoothly (spherically, shortest path on a quaternion sphere) 00443 * interpolates between two UNIT quaternion positions 00444 * slerp(p, q, t) = ( p*sin((1 - t)*omega) + q*sin(t*omega) ) / sin(omega) 00445 */ 00446 inline void slerp(const quaternion4<T> &to, const T &t) 00447 { 00448 /* Temporary variables */ 00449 T to1[4]; 00450 T omega, cosom, sinom; 00451 T scale0, scale1; 00452 00453 /* Calculate cosine */ 00454 cosom = X*to.X + Y*to.Y + Z*to.Z + W*to.W; 00455 00456 /* Adjust signs (if necessary) */ 00457 if (cosom < T(0)) 00458 { 00459 cosom = -cosom; 00460 to1[0] = -to.X; 00461 to1[1] = -to.Y; 00462 to1[2] = -to.Z; 00463 to1[3] = -to.W; 00464 } 00465 else 00466 { 00467 to1[0] = to.X; 00468 to1[1] = to.Y; 00469 to1[2] = to.Z; 00470 to1[3] = to.W; 00471 } 00472 00473 /* Calculate coefficients */ 00474 if ( ( T(1) - cosom ) > T(1e-10) ) 00475 { 00476 /* Standard case (slerp) */ 00477 omega = acos(cosom); 00478 sinom = sin(omega); 00479 scale0 = sin((T(1) - t) * omega) / sinom; 00480 scale1 = sin(t * omega) / sinom; 00481 } 00482 else 00483 { 00484 /* 00485 * "from" and "to" quaternions are very close 00486 * ... so we can do a linear interpolation 00487 */ 00488 scale0 = T(1) - t; 00489 scale1 = t; 00490 } 00491 00492 /* Calculate final values */ 00493 X = scale0*X + scale1*to1[0]; 00494 Y = scale0*Y + scale1*to1[1]; 00495 Z = scale0*Z + scale1*to1[2]; 00496 W = scale0*W + scale1*to1[3]; 00497 } 00498 00499 inline void slerp(const quaternion4<T> &from, const quaternion4<T> &to, const T &t) 00500 { 00501 /* Temporary variables */ 00502 T to1[4]; 00503 T omega, cosom, sinom; 00504 T scale0, scale1; 00505 00506 /* Calculate cosine */ 00507 cosom = from.X*to.X + from.Y*to.Y + from.Z*to.Z + from.W*to.W; 00508 00509 /* Adjust signs (if necessary) */ 00510 if (cosom < T(0)) 00511 { 00512 cosom = -cosom; 00513 to1[0] = -to.X; 00514 to1[1] = -to.Y; 00515 to1[2] = -to.Z; 00516 to1[3] = -to.W; 00517 } 00518 else 00519 { 00520 to1[0] = to.X; 00521 to1[1] = to.Y; 00522 to1[2] = to.Z; 00523 to1[3] = to.W; 00524 } 00525 00526 /* Calculate coefficients */ 00527 if ((T(1) - cosom) > T(1e-10)) 00528 { 00529 /* Standard case (slerp) */ 00530 omega = acos(cosom); 00531 sinom = sin(omega); 00532 scale0 = sin((T(1) - t) * omega) / sinom; 00533 scale1 = sin(t * omega) / sinom; 00534 } 00535 else 00536 { 00537 /* 00538 * "from" and "to" quaternions are very close 00539 * ... so we can do a linear interpolation 00540 */ 00541 scale0 = T(1) - t; 00542 scale1 = t; 00543 } 00544 00545 /* Calculate final values */ 00546 X = scale0*from.X + scale1*to1[0]; 00547 Y = scale0*from.Y + scale1*to1[1]; 00548 Z = scale0*from.Z + scale1*to1[2]; 00549 W = scale0*from.W + scale1*to1[3]; 00550 } 00551 00552 inline void reset() // Load identity 00553 { 00554 X = Y = Z = T(0); 00555 W = T(1); 00556 } 00557 00558 /* === Members === */ 00559 00560 T X, Y, Z, W; 00561 00562 }; 00563 00564 00565 typedef quaternion4<f32> quaternion; //<! for backwards compatibility. 00566 typedef quaternion4<f32> quaternion4f; 00567 typedef quaternion4<f64> quaternion4d; 00568 00569 00570 } // /namespace dim 00571 00572 } // /namespace sp 00573 00574 00575 #endif 00576 00577 00578 00579 // ================================================================================
