00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 #ifndef NV_QUATERNION_H
00053 #define NV_QUATERNION_H
00054
00055 #include <NvFoundation.h>
00056
00059
00060 namespace nv {
00061
00062 template <class T> class vec2;
00063 template <class T> class vec3;
00064 template <class T> class vec4;
00065
00067
00068
00069
00071
00072 template< class T>
00073 class quaternion
00074 {
00075 public:
00076
00077 quaternion() : x(0.0), y(0.0), z(0.0), w(0.0)
00078 {
00079 }
00080
00081 quaternion( const T v[4] )
00082 {
00083 set_value( v );
00084 }
00085
00086
00087 quaternion( T q0, T q1, T q2, T q3 )
00088 {
00089 set_value( q0, q1, q2, q3 );
00090 }
00091
00092
00093 quaternion( const matrix4<T> & m )
00094 {
00095 set_value( m );
00096 }
00097
00098
00099 quaternion( const vec3<T> &axis, T radians )
00100 {
00101 set_value( axis, radians );
00102 }
00103
00104
00105 quaternion( const vec3<T> &rotateFrom, const vec3<T> &rotateTo )
00106 {
00107 set_value( rotateFrom, rotateTo );
00108 }
00109
00110 quaternion( const vec3<T> & from_look, const vec3<T> & from_up,
00111 const vec3<T>& to_look, const vec3<T>& to_up)
00112 {
00113 set_value(from_look, from_up, to_look, to_up);
00114 }
00115
00116 const T * get_value() const
00117 {
00118
00119 return &_array[0];
00120 }
00121
00122 void get_value( T &q0, T &q1, T &q2, T &q3 ) const
00123 {
00124
00125
00126
00127
00128
00129 q0 = _array[0];
00130 q1 = _array[1];
00131 q2 = _array[2];
00132 q3 = _array[3];
00133 }
00134
00135 quaternion & set_value( T q0, T q1, T q2, T q3 )
00136 {
00137 _array[0] = q0;
00138 _array[1] = q1;
00139 _array[2] = q2;
00140 _array[3] = q3;
00141 return *this;
00142 }
00143
00144 void get_value( vec3<T> &axis, T &radians ) const
00145 {
00146 radians = T(acos( _array[3] ) * T(2.0));
00147 if ( radians == T(0.0) )
00148 axis = vec3<T>( 0.0, 0.0, 1.0 );
00149 else
00150 {
00151 axis[0] = _array[0];
00152 axis[1] = _array[1];
00153 axis[2] = _array[2];
00154 axis = normalize(axis);
00155 }
00156 }
00157
00158 void get_value( matrix4<T> & m ) const
00159 {
00160 T s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00161
00162 T norm = _array[0] * _array[0] + _array[1] * _array[1] + _array[2] * _array[2] + _array[3] * _array[3];
00163
00164 s = ( norm == T(0.0)) ? T(0.0) : ( T(2.0) / norm );
00165
00166 xs = _array[0] * s;
00167 ys = _array[1] * s;
00168 zs = _array[2] * s;
00169
00170 wx = _array[3] * xs;
00171 wy = _array[3] * ys;
00172 wz = _array[3] * zs;
00173
00174 xx = _array[0] * xs;
00175 xy = _array[0] * ys;
00176 xz = _array[0] * zs;
00177
00178 yy = _array[1] * ys;
00179 yz = _array[1] * zs;
00180 zz = _array[2] * zs;
00181
00182 m(0,0) = T( T(1.0) - ( yy + zz ));
00183 m(1,0) = T ( xy + wz );
00184 m(2,0) = T ( xz - wy );
00185
00186 m(0,1) = T ( xy - wz );
00187 m(1,1) = T ( T(1.0) - ( xx + zz ));
00188 m(2,1) = T ( yz + wx );
00189
00190 m(0,2) = T ( xz + wy );
00191 m(1,2) = T ( yz - wx );
00192 m(2,2) = T ( T(1.0) - ( xx + yy ));
00193
00194 m(3,0) = m(3,1) = m(3,2) = m(0,3) = m(1,3) = m(2,3) = T(0.0);
00195 m(3,3) = T(1.0);
00196 }
00197
00198 quaternion & set_value( const T * qp )
00199 {
00200 for ( int32_t i = 0; i < 4; i++) _array[i] = qp[i];
00201
00202 return *this;
00203 }
00204
00205 quaternion & set_value( const matrix4<T> & m )
00206 {
00207 T tr, s;
00208 int32_t i, j, k;
00209 const int32_t nxt[3] = { 1, 2, 0 };
00210
00211 tr = m(0,0) + m(1,1) + m(2,2);
00212
00213 if ( tr > T(0) )
00214 {
00215 s = T(sqrt( tr + m(3,3) ));
00216 _array[3] = T ( s * 0.5 );
00217 s = T(0.5) / s;
00218
00219 _array[0] = T ( ( m(1,2) - m(2,1) ) * s );
00220 _array[1] = T ( ( m(2,0) - m(0,2) ) * s );
00221 _array[2] = T ( ( m(0,1) - m(1,0) ) * s );
00222 }
00223 else
00224 {
00225 i = 0;
00226 if ( m(1,1) > m(0,0) )
00227 i = 1;
00228
00229 if ( m(2,2) > m(i,i) )
00230 i = 2;
00231
00232 j = nxt[i];
00233 k = nxt[j];
00234
00235 s = T(sqrt( ( m(i,j) - ( m(j,j) + m(k,k) )) + T(1.0) ));
00236
00237 _array[i] = T ( s * 0.5 );
00238 s = T(0.5 / s);
00239
00240 _array[3] = T ( ( m(j,k) - m(k,j) ) * s );
00241 _array[j] = T ( ( m(i,j) + m(j,i) ) * s );
00242 _array[k] = T ( ( m(i,k) + m(k,i) ) * s );
00243 }
00244
00245 return *this;
00246 }
00247
00248 quaternion & set_value( const vec3<T> &axis, T theta )
00249 {
00250 T sqnorm = square_norm(axis);
00251
00252 if (sqnorm == T(0.0))
00253 {
00254
00255 x = y = z = T(0.0);
00256 w = T(1.0);
00257 }
00258 else
00259 {
00260 theta *= T(0.5);
00261 T sin_theta = T(sin(theta));
00262
00263 if ( sqnorm != T(1))
00264 sin_theta /= T(sqrt(sqnorm));
00265 x = sin_theta * axis[0];
00266 y = sin_theta * axis[1];
00267 z = sin_theta * axis[2];
00268 w = T(cos(theta));
00269 }
00270 return *this;
00271 }
00272
00273 quaternion & set_value( const vec3<T> & rotateFrom, const vec3<T> & rotateTo )
00274 {
00275 vec3<T> p1, p2;
00276 T alpha;
00277
00278 p1 = normalize(rotateFrom);
00279 p2 = normalize(rotateTo);
00280
00281 alpha = dot( p1, p2);
00282
00283 if( alpha == T(1.0) ) {
00284 *this = quaternion();
00285 return *this;
00286 }
00287
00288
00289 if( alpha == T(-1.0))
00290 {
00291 vec3<T> v;
00292
00293 if(p1[0] != p1[1] || p1[0] != p1[2])
00294 v = vec3<T>(p1[1], p1[2], p1[0]);
00295 else
00296 v = vec3<T>(-p1[0], p1[1], p1[2]);
00297
00298 v -= p1 * dot( p1, v);
00299 v = normalize(v);
00300
00301 set_value(v, T(3.1415926));
00302 return *this;
00303 }
00304
00305 p1 = normalize( cross( p1, p2));
00306
00307 set_value(p1,T(acos(alpha)));
00308
00309 return *this;
00310 }
00311
00312 quaternion & set_value( const vec3<T> & from_look, const vec3<T> & from_up,
00313 const vec3<T> & to_look, const vec3<T> & to_up)
00314 {
00315 quaternion r_look = quaternion(from_look, to_look);
00316
00317 vec3<T> rotated_from_up(from_up);
00318 r_look.mult_vec(rotated_from_up);
00319
00320 quaternion r_twist = quaternion(rotated_from_up, to_up);
00321
00322 *this = r_twist;
00323 *this *= r_look;
00324 return *this;
00325 }
00326
00327 quaternion & operator *= ( const quaternion<T> & qr ) {
00328 quaternion ql(*this);
00329
00330 w = ql.w * qr.w - ql.x * qr.x - ql.y * qr.y - ql.z * qr.z;
00331 x = ql.w * qr.x + ql.x * qr.w + ql.y * qr.z - ql.z * qr.y;
00332 y = ql.w * qr.y + ql.y * qr.w + ql.z * qr.x - ql.x * qr.z;
00333 z = ql.w * qr.z + ql.z * qr.w + ql.x * qr.y - ql.y * qr.x;
00334
00335 return *this;
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348 void mult_vec( const vec3<T> &src, vec3<T> &dst ) const
00349 {
00350 T v_coef = w * w - x * x - y * y - z * z;
00351 T u_coef = T(2.0) * (src[0] * x + src[1] * y + src[2] * z);
00352 T c_coef = T(2.0) * w;
00353
00354 dst._array[0] = v_coef * src._array[0] + u_coef * x + c_coef * (y * src._array[2] - z * src._array[1]);
00355 dst._array[1] = v_coef * src._array[1] + u_coef * y + c_coef * (z * src._array[0] - x * src._array[2]);
00356 dst._array[2] = v_coef * src._array[2] + u_coef * z + c_coef * (x * src._array[1] - y * src._array[0]);
00357 }
00358
00359 void mult_vec( vec3<T> & src_and_dst) const
00360 {
00361 mult_vec(vec3<T>(src_and_dst), src_and_dst);
00362 }
00363
00364 void scale_angle( T scaleFactor ) {
00365 vec3<T> axis;
00366 T radians;
00367
00368 get_value(axis, radians);
00369 radians *= scaleFactor;
00370 set_value(axis, radians);
00371 }
00372
00373
00374
00375 T & operator []( int32_t i ) {
00376 return _array[i];
00377 }
00378
00379 const T & operator []( int32_t i ) const {
00380 return _array[i];
00381 }
00382
00383
00384 friend bool operator == ( const quaternion<T> & lhs, const quaternion<T> & rhs ) {
00385 bool r = true;
00386 for (int32_t i = 0; i < 4; i++)
00387 r &= lhs._array[i] == rhs._array[i];
00388 return r;
00389 }
00390
00391 friend bool operator != ( const quaternion<T> & lhs, const quaternion<T> & rhs ) {
00392 bool r = true;
00393 for (int32_t i = 0; i < 4; i++)
00394 r &= lhs._array[i] == rhs._array[i];
00395 return r;
00396 }
00397
00398 friend quaternion<T> operator * ( const quaternion<T> & lhs, const quaternion<T> & rhs ) {
00399 quaternion r(lhs);
00400 r *= rhs;
00401 return r;
00402 }
00403
00404
00405 union
00406 {
00407 struct
00408 {
00409 T x;
00410 T y;
00411 T z;
00412 T w;
00413 };
00414 T _array[4];
00415 };
00416
00417 };
00418
00419 template<class T>
00420 inline quaternion<T> normalize( const quaternion<T> &q) {
00421 quaternion<T> r(q);
00422 T rnorm = T(1.0) / T(sqrt( q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z));
00423
00424 r.x *= rnorm;
00425 r.y *= rnorm;
00426 r.z *= rnorm;
00427 r.w *= rnorm;
00428
00429 return r;
00430 }
00431
00432 template<class T>
00433 quaternion<T> conjugate( const quaternion<T> & q) {
00434 quaternion<T> r(q);
00435 r._array[0] *= T(-1.0);
00436 r._array[1] *= T(-1.0);
00437 r._array[2] *= T(-1.0);
00438 return r;
00439 }
00440
00441 template<class T>
00442 quaternion<T> inverse( const quaternion<T> & q) {
00443 return conjugate(q);
00444 }
00445
00446 template<class T>
00447 quaternion<T> slerp( const quaternion<T> & p, const quaternion<T> & q, T alpha ) {
00448 quaternion<T> r;
00449
00450 T cos_omega = p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w;
00451
00452
00453 int32_t bflip;
00454 if ( ( bflip = (cos_omega < T(0))) )
00455 cos_omega = -cos_omega;
00456
00457
00458 T beta = T(1) - alpha;
00459
00460 if(cos_omega >= T(1))
00461 return p;
00462
00463 T omega = T(acos(cos_omega));
00464 T one_over_sin_omega = T(1.0) / T(sin(omega));
00465
00466 beta = T(sin(omega*beta) * one_over_sin_omega);
00467 alpha = T(sin(omega*alpha) * one_over_sin_omega);
00468
00469 if (bflip)
00470 alpha = -alpha;
00471
00472 r.x = beta * p._array[0]+ alpha * q._array[0];
00473 r.y = beta * p._array[1]+ alpha * q._array[1];
00474 r.z = beta * p._array[2]+ alpha * q._array[2];
00475 r.w = beta * p._array[3]+ alpha * q._array[3];
00476 return r;
00477 }
00478
00479 };
00480
00481 #endif