00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef Quaternion_INCLUDE_ONCE
00033 #define Quaternion_INCLUDE_ONCE
00034
00035 #include <vlCore/glsl_math.hpp>
00036
00037 namespace vl
00038 {
00039
00040
00041
00043 template<typename T_Scalar>
00044 class Quaternion
00045 {
00046 public:
00047 typedef T_Scalar scalar_type;
00048
00050 Quaternion()
00051 {
00052 setNoRotation();
00053 }
00054
00056 template<typename T>
00057 explicit Quaternion(const Quaternion<T>& quat)
00058 {
00059 mXYZW.x() = (T_Scalar)quat.xyzw().x();
00060 mXYZW.y() = (T_Scalar)quat.xyzw().y();
00061 mXYZW.z() = (T_Scalar)quat.xyzw().z();
00062 mXYZW.w() = (T_Scalar)quat.xyzw().w();
00063 }
00064
00066 explicit Quaternion(T_Scalar x, T_Scalar y, T_Scalar z, T_Scalar w)
00067 {
00068 mXYZW.x() = x;
00069 mXYZW.y() = y;
00070 mXYZW.z() = z;
00071 mXYZW.w() = w;
00072 }
00073
00075 explicit Quaternion(T_Scalar degrees, const Vector3<T_Scalar>& axis)
00076 {
00077 setFromAxisAngle(axis, degrees);
00078 }
00079
00081 explicit Quaternion(const Vector4<T_Scalar>& v)
00082 {
00083 mXYZW.x() = (T_Scalar)v.x();
00084 mXYZW.y() = (T_Scalar)v.y();
00085 mXYZW.z() = (T_Scalar)v.z();
00086 mXYZW.w() = (T_Scalar)v.w();
00087 }
00088
00090 Quaternion& operator=(const Quaternion& q)
00091 {
00092 mXYZW.x() = q.x();
00093 mXYZW.y() = q.y();
00094 mXYZW.z() = q.z();
00095 mXYZW.w() = q.w();
00096 return *this;
00097 }
00098
00100 Quaternion& operator=(const Vector4<T_Scalar>& v)
00101 {
00102 mXYZW.x() = (T_Scalar)v.x();
00103 mXYZW.y() = (T_Scalar)v.y();
00104 mXYZW.z() = (T_Scalar)v.z();
00105 mXYZW.w() = (T_Scalar)v.w();
00106 return *this;
00107 }
00108
00109 bool operator==(const Quaternion& q) const
00110 {
00111 return x() == q.x() && y() == q.y() && z() == q.z() && w() == q.w();
00112 }
00113
00114 bool operator!=(const Quaternion& q) const
00115 {
00116 return !operator==(q);
00117 }
00118
00120 bool operator<(const Quaternion& other) const
00121 {
00122 if (x() != other.x())
00123 return x() < other.x();
00124 if (y() != other.y())
00125 return y() < other.y();
00126 if (z() != other.z())
00127 return z() < other.z();
00128 else
00129 return w() < other.w();
00130 }
00131
00133 const Vector4<T_Scalar>& xyzw() const { return mXYZW; }
00134
00136 Vector4<T_Scalar>& xyzw() { return mXYZW; }
00137
00138 T_Scalar& x() { return mXYZW.x(); }
00139
00140 T_Scalar& y() { return mXYZW.y(); }
00141
00142 T_Scalar& z() { return mXYZW.z(); }
00143
00144 T_Scalar& w() { return mXYZW.w(); }
00145
00146 const T_Scalar& x() const { return mXYZW.x(); }
00147
00148 const T_Scalar& y() const { return mXYZW.y(); }
00149
00150 const T_Scalar& z() const { return mXYZW.z(); }
00151
00152 const T_Scalar& w() const { return mXYZW.w(); }
00153
00154 Quaternion operator*(T_Scalar val) const
00155 {
00156 Quaternion t = *this;
00157 t.x() *= val;
00158 t.y() *= val;
00159 t.z() *= val;
00160 t.w() *= val;
00161 return t;
00162 }
00163
00164 Quaternion& operator*=(T_Scalar val)
00165 {
00166 x() *= val;
00167 y() *= val;
00168 z() *= val;
00169 w() *= val;
00170 return *this;
00171 }
00172
00173 Quaternion operator/(T_Scalar val) const
00174 {
00175 Quaternion t = *this;
00176 val = (T_Scalar)1.0 / val;
00177 t.x() *= val;
00178 t.y() *= val;
00179 t.z() *= val;
00180 t.w() *= val;
00181 return t;
00182 }
00183
00184 Quaternion& operator/=(T_Scalar val)
00185 {
00186 val = (T_Scalar)1.0 / val;
00187 x() *= val;
00188 y() *= val;
00189 z() *= val;
00190 w() *= val;
00191 return *this;
00192 }
00193
00194 Quaternion operator+(const Quaternion& q) const
00195 {
00196 Quaternion t = *this;
00197 t.x() += q.x();
00198 t.y() += q.y();
00199 t.z() += q.z();
00200 t.w() += q.w();
00201 return t;
00202 }
00203
00204 Quaternion& operator+=(const Quaternion& q)
00205 {
00206 x() += q.x();
00207 y() += q.y();
00208 z() += q.z();
00209 w() += q.w();
00210 return *this;
00211 }
00212
00213 Quaternion operator-(const Quaternion& q) const
00214 {
00215 Quaternion t = *this;
00216 t.x() -= q.x();
00217 t.y() -= q.y();
00218 t.z() -= q.z();
00219 t.w() -= q.w();
00220 return t;
00221 }
00222
00223 Quaternion& operator-=(const Quaternion& q)
00224 {
00225 x() -= q.x();
00226 y() -= q.y();
00227 z() -= q.z();
00228 w() -= q.w();
00229 return *this;
00230 }
00231
00233 Quaternion operator-() const
00234 {
00235 return Quaternion(-x(), -y(), -z(), -w());
00236 }
00237
00239 Quaternion& setZero()
00240 {
00241 mXYZW.x() = 0;
00242 mXYZW.y() = 0;
00243 mXYZW.z() = 0;
00244 mXYZW.w() = 0;
00245 return *this;
00246 }
00247
00249 static Quaternion getZero()
00250 {
00251 return Quaternion().setZero();
00252 }
00253
00255 static Quaternion& getZero(Quaternion& q)
00256 {
00257 return q.setZero();
00258 }
00259
00261 Quaternion& setNoRotation()
00262 {
00263 mXYZW.x() = 0;
00264 mXYZW.y() = 0;
00265 mXYZW.z() = 0;
00266 mXYZW.w() = 1;
00267 return *this;
00268 }
00269
00271 static Quaternion getNoRotation()
00272 {
00273 return Quaternion();
00274 }
00275
00277 static Quaternion& getNoRotation(Quaternion& q)
00278 {
00279 return q.setNoRotation();
00280 }
00281
00283 Quaternion& setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to);
00284
00286 static Quaternion getFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
00287 {
00288 return Quaternion().setFromVectors(from, to);
00289 }
00290
00292 static Quaternion& getFromVectors(Quaternion& q, const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
00293 {
00294 return q.setFromVectors(from, to);
00295 }
00296
00299 Quaternion& setFromMatrix(const Matrix4<T_Scalar>& m);
00300
00302 static Quaternion getFromMatrix(const Matrix4<T_Scalar>& m)
00303 {
00304 return Quaternion().setFromMatrix(m);
00305 }
00306
00308 static Quaternion& getFromMatrix(Quaternion& q, const Matrix4<T_Scalar>& m)
00309 {
00310 return q.setFromMatrix(m);
00311 }
00312
00315 Quaternion& setFromMatrix(const Matrix3<T_Scalar>& m);
00316
00318 static Quaternion getFromMatrix(const Matrix3<T_Scalar>& m)
00319 {
00320 return Quaternion().setFromMatrix(m);
00321 }
00322
00324 static Quaternion& getFromMatrix(Quaternion& q, const Matrix3<T_Scalar>& m)
00325 {
00326 return q.setFromMatrix(m);
00327 }
00328
00329 Quaternion& setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ);
00330
00331 static Quaternion getFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ)
00332 {
00333 return Quaternion().setFromEulerXYZ(degX, degY, degZ);
00334 }
00335
00336 static Quaternion& getFromEulerXYZ(Quaternion& q, T_Scalar degX, T_Scalar degY, T_Scalar degZ)
00337 {
00338 return q.setFromEulerXYZ(degX, degY, degZ);
00339 }
00340
00341 Quaternion& setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX);
00342
00343 static Quaternion getFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX)
00344 {
00345 return Quaternion().setFromEulerZYX(degZ, degY, degX);
00346 }
00347
00348 static Quaternion& getFromEulerZYX(Quaternion& q, T_Scalar degZ, T_Scalar degY, T_Scalar degX)
00349 {
00350 return q.setFromEulerZYX(degZ, degY, degX);
00351 }
00352
00353 Quaternion& setFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees);
00354
00355 static Quaternion getFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees)
00356 {
00357 return Quaternion().setFromAxisAngle(axis, degrees);
00358 }
00359
00360 static Quaternion& getFromAxisAngle(Quaternion& q, const Vector3<T_Scalar>& axis, T_Scalar degrees)
00361 {
00362 return q.setFromAxisAngle(axis, degrees);
00363 }
00364
00366 void toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const;
00367
00369 Matrix4<T_Scalar> toMatrix4() const;
00370
00372 Matrix4<T_Scalar>& toMatrix4(Matrix4<T_Scalar>&) const;
00373
00375 Matrix3<T_Scalar> toMatrix3() const;
00376
00378 Matrix3<T_Scalar>& toMatrix3(Matrix3<T_Scalar>&) const;
00379
00381 T_Scalar dot(const Quaternion& q) const
00382 {
00383 return x()*q.x() + y()*q.y() + z()*q.z() + w()*q.w();
00384 }
00385
00387 T_Scalar length() const { return mXYZW.length(); }
00388
00391 Quaternion& normalize(T_Scalar* len=NULL) { mXYZW.normalize(len); return *this; }
00392
00395 Quaternion getNormalized(T_Scalar* len=NULL) const { Quaternion t = *this; t.normalize(len); return t; }
00396
00399 Quaternion& getNormalized(Quaternion& q, T_Scalar* len=NULL) const { q = *this; q.normalize(len); return q; }
00400
00402 T_Scalar lengthSquared() const
00403 {
00404 return x()*x() + y()*y() + z()*z() + w()*w();
00405 }
00406
00408 Quaternion getConjugate() const
00409 {
00410 return Quaternion(-x(), -y(), -z(), w());
00411 }
00412
00414 Quaternion& getConjugate(Quaternion& q) const
00415 {
00416 q = Quaternion(-x(), -y(), -z(), w());
00417 return q;
00418 }
00419
00421 Quaternion getInverse() const
00422 {
00423 return getConjugate() / lengthSquared();
00424 }
00425
00427 Quaternion& getInverse(Quaternion& q) const
00428 {
00429 q = getConjugate() / lengthSquared();
00430 return q;
00431 }
00432
00436 static Quaternion getSlerp(T_Scalar t, const Quaternion& a, const Quaternion& b);
00437
00441 static Quaternion& getSlerp(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b);
00442
00444 static Quaternion getSquad(T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
00445 {
00446 return getSlerp((T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
00447 }
00448
00450 static Quaternion& getSquad(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
00451 {
00452 return getSlerp(out, (T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
00453 }
00454
00458 static Quaternion getNlerp( T_Scalar t, const Quaternion& a, const Quaternion& b )
00459 {
00460 Quaternion q = a + (b - a) * t;
00461 q.normalize();
00462 return q;
00463 }
00464
00468 static Quaternion& getNlerp( Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b )
00469 {
00470 out = a + (b - a) * t;
00471 out.normalize();
00472 return out;
00473 }
00474
00475
00476 protected:
00477 Vector4<T_Scalar> mXYZW;
00478 };
00479
00480 template<typename T_Scalar>
00481 inline Quaternion<T_Scalar> operator*(T_Scalar r, const Quaternion<T_Scalar>& q)
00482 {
00483 return q * r;
00484 }
00485
00486 template<typename T_Scalar>
00487 inline Quaternion<T_Scalar> operator*(const Quaternion<T_Scalar>& q1, const Quaternion<T_Scalar>& q2)
00488 {
00489 Quaternion<T_Scalar> q;
00490 q.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y();
00491 q.y() = q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z();
00492 q.z() = q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x();
00493 q.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
00494 return q;
00495 }
00496
00497
00498 template<typename T_Scalar>
00499 inline Vector3<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector3<T_Scalar>& v)
00500 {
00501
00502 T_Scalar x2 = q.x() * q.x();
00503 T_Scalar y2 = q.y() * q.y();
00504 T_Scalar z2 = q.z() * q.z();
00505 T_Scalar xy = q.x() * q.y();
00506 T_Scalar xz = q.x() * q.z();
00507 T_Scalar yz = q.y() * q.z();
00508 T_Scalar wx = q.w() * q.x();
00509 T_Scalar wy = q.w() * q.y();
00510 T_Scalar wz = q.w() * q.z();
00511
00512 Vector3<T_Scalar> r;
00513 r.x() = ( v.x()*(1.0f - 2.0f * (y2 + z2)) + v.y()*(2.0f * (xy - wz)) + v.z()*(2.0f * (xz + wy)) );
00514 r.y() = ( v.x()*(2.0f * (xy + wz)) + v.y()*(1.0f - 2.0f * (x2 + z2)) + v.z()*(2.0f * (yz - wx)) );
00515 r.z() = ( v.x()*(2.0f * (xz - wy)) + v.y()*(2.0f * (yz + wx)) + v.z()*(1.0f - 2.0f * (x2 + y2)) );
00516 return r;
00517 }
00518
00519
00520 template<typename T_Scalar>
00521 inline Vector4<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector4<T_Scalar>& v)
00522 {
00523 return Vector4<T_Scalar>( q * v.xyz(), v.w() );
00524 }
00525
00526 template<typename T_Scalar>
00527 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ )
00528 {
00529 *this = Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1));
00530 return *this;
00531 }
00532
00533 template<typename T_Scalar>
00534 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX )
00535 {
00536 *this = Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0));
00537 return *this;
00538 }
00539
00540 template<typename T_Scalar>
00541 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix4<T_Scalar>& m)
00542 {
00543 return setFromMatrix( m.get3x3() );
00544 }
00545
00546 template<typename T_Scalar>
00547 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix3<T_Scalar>& m)
00548 {
00549 T_Scalar tr, s, q[4];
00550
00551 int next[3] = {1, 2, 0};
00552
00553 tr = m.e(0,0) + m.e(1,1) + m.e(2,2);
00554
00555
00556 if (tr + (T_Scalar)1.0 > 0.0)
00557 {
00558 s = vl::sqrt(tr + (T_Scalar)1.0);
00559 w() = s / (T_Scalar)2.0;
00560 s = (T_Scalar)0.5 / s;
00561 x() = (m.e(2,1) - m.e(1,2)) * s;
00562 y() = (m.e(0,2) - m.e(2,0)) * s;
00563 z() = (m.e(1,0) - m.e(0,1)) * s;
00564 }
00565 else
00566 {
00567
00568 int i, j, k;
00569 i = 0;
00570 if (m.e(1,1) > m.e(0,0)) i = 1;
00571 if (m.e(2,2) > m.e(i,i)) i = 2;
00572 j = next[i];
00573 k = next[j];
00574
00575 s = vl::sqrt((m.e(i,i) - (m.e(j,j) + m.e(k,k))) + (T_Scalar)1.0);
00576
00577 q[i] = s * (T_Scalar)0.5;
00578
00579 if (s != 0.0)
00580 s = (T_Scalar)0.5 / s;
00581
00582 q[3] = (m.e(k,j) - m.e(j,k)) * s;
00583 q[j] = (m.e(j,i) + m.e(i,j)) * s;
00584 q[k] = (m.e(k,i) + m.e(i,k)) * s;
00585
00586 x() = q[0];
00587 y() = q[1];
00588 z() = q[2];
00589 w() = q[3];
00590 }
00591
00592 return *this;
00593 }
00594
00595 template<typename T_Scalar>
00596 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromAxisAngle( const Vector3<T_Scalar>& axis, T_Scalar degrees )
00597 {
00598 degrees *= (T_Scalar)dDEG_TO_RAD;
00599 T_Scalar sa2 = sin(degrees * (T_Scalar)0.5);
00600 Vector3<T_Scalar> na = axis;
00601 na.normalize();
00602 mXYZW.x() = na.x() * sa2;
00603 mXYZW.y() = na.y() * sa2;
00604 mXYZW.z() = na.z() * sa2;
00605 mXYZW.w() = cos(degrees * (T_Scalar)0.5);
00606 return *this;
00607 }
00608
00609 template<typename T_Scalar>
00610 void Quaternion<T_Scalar>::toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const
00611 {
00612 T_Scalar iscale = sqrt( x()*x() + y()*y() + z()*z() );
00613 if (iscale == 0)
00614 {
00615 axis.x() = 0;
00616 axis.y() = 0;
00617 axis.z() = 0;
00618 degrees = 0;
00619 }
00620 else
00621 {
00622 iscale = T_Scalar(1.0) / iscale;
00623 axis.x() = x() * iscale;
00624 axis.y() = y() * iscale;
00625 axis.z() = z() * iscale;
00626 VL_CHECK(w()>=-1.0 && w()<=+1.0)
00627 T_Scalar tw = clamp(w(),(T_Scalar)-1.0,(T_Scalar)+1.0);
00628 degrees = acos( tw ) * (T_Scalar)2.0 * (T_Scalar)dRAD_TO_DEG;
00629 }
00630 }
00631
00632 template<typename T_Scalar>
00633 Matrix4<T_Scalar>& Quaternion<T_Scalar>::toMatrix4(Matrix4<T_Scalar>& out) const
00634 {
00635 T_Scalar x2 = x() * x();
00636 T_Scalar y2 = y() * y();
00637 T_Scalar z2 = z() * z();
00638 T_Scalar xy = x() * y();
00639 T_Scalar xz = x() * z();
00640 T_Scalar yz = y() * z();
00641 T_Scalar wx = w() * x();
00642 T_Scalar wy = w() * y();
00643 T_Scalar wz = w() * z();
00644
00645 return out = Matrix4<T_Scalar>(
00646 (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy - wz)), (2.0f * (xz + wy)), 0.0f,
00647 (2.0f * (xy + wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz - wx)), 0.0f,
00648 (2.0f * (xz - wy)), (2.0f * (yz + wx)), (1.0f - 2.0f * (x2 + y2)), 0.0f,
00649 0.0f, 0.0f, 0.0f, 1.0f );
00650 }
00651
00652 template<typename T_Scalar>
00653 Matrix4<T_Scalar> Quaternion<T_Scalar>::toMatrix4() const
00654 {
00655 Matrix4<T_Scalar> out;
00656 return toMatrix4(out);
00657 }
00658
00659 template<typename T_Scalar>
00660 Matrix3<T_Scalar>& Quaternion<T_Scalar>::toMatrix3(Matrix3<T_Scalar>& out) const
00661 {
00662 T_Scalar x2 = x() * x();
00663 T_Scalar y2 = y() * y();
00664 T_Scalar z2 = z() * z();
00665 T_Scalar xy = x() * y();
00666 T_Scalar xz = x() * z();
00667 T_Scalar yz = y() * z();
00668 T_Scalar wx = w() * x();
00669 T_Scalar wy = w() * y();
00670 T_Scalar wz = w() * z();
00671
00672 return out = Matrix3<T_Scalar>(
00673 (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy + wz)), (2.0f * (xz - wy)),
00674 (2.0f * (xy - wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz + wx)),
00675 (2.0f * (xz + wy)), (2.0f * (yz - wx)), (1.0f - 2.0f * (x2 + y2)) );
00676 }
00677
00678 template<typename T_Scalar>
00679 Matrix3<T_Scalar> Quaternion<T_Scalar>::toMatrix3() const
00680 {
00681 Matrix3<T_Scalar> out;
00682 return toMatrix3(out);
00683 }
00684
00685 template<typename T_Scalar>
00686 Quaternion<T_Scalar> Quaternion<T_Scalar>::getSlerp( T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b )
00687 {
00688 Quaternion<T_Scalar> q;
00689 getSlerp(q, t, a, b);
00690 return q;
00691 }
00692
00693 template<typename T_Scalar>
00694 Quaternion<T_Scalar>& Quaternion<T_Scalar>::getSlerp( Quaternion<T_Scalar>& out, T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b )
00695 {
00696 T_Scalar scale_a, scale_b, omega, sinom;
00697 T_Scalar cosom = a.dot(b);
00698
00699 Quaternion<T_Scalar> b2(b);
00700
00701 if ( cosom < 0 )
00702 {
00703 cosom = -cosom;
00704 b2 = -b;
00705 }
00706
00707
00708 cosom = cosom > (T_Scalar)1 ? (T_Scalar)1 : cosom;
00709
00710 if( cosom < (T_Scalar)1.0 )
00711 {
00712 omega = acos(cosom);
00713 sinom = sin(omega);
00714 VL_CHECK(sinom != 0)
00715 scale_a = sin(((T_Scalar)1.0-t) * omega) / sinom;
00716 scale_b = sin(t * omega) / sinom;
00717 }
00718 else
00719 {
00720
00721 scale_a = (T_Scalar)1.0 - t;
00722 scale_b = t;
00723 }
00724
00725 return out = (a*scale_a) + (b2*scale_b);
00726 }
00727
00728 template<typename T_Scalar>
00729 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
00730 {
00731 Vector3<T_Scalar> a,b;
00732 a = from;
00733 b = to;
00734 a.normalize();
00735 b.normalize();
00736 Vector3<T_Scalar> axis = cross(a,b);
00737 T_Scalar len = 0;
00738 axis.normalize(&len);
00739 if(len)
00740 {
00741 T_Scalar cosa = vl::dot(a,b);
00742 cosa = clamp(cosa, (T_Scalar)-1.0, (T_Scalar)+1.0);
00743 T_Scalar alpha = acos( cosa );
00744 setFromAxisAngle(axis, alpha*(T_Scalar)dRAD_TO_DEG);
00745 }
00746 else
00747 setNoRotation();
00748 return *this;
00749 }
00750
00751 typedef Quaternion<float> fquat;
00752 typedef Quaternion<double> dquat;
00753 typedef Quaternion<real> quat;
00754
00755 }
00756
00757 #endif