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
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106 #ifndef QUATERNION_H
00107 #define QUATERNION_H
00108
00109 #include <vcg/space/point3.h>
00110 #include <vcg/space/point4.h>
00111 #include <vcg/math/base.h>
00112 #include <vcg/math/matrix44.h>
00113 #include <vcg/math/matrix33.h>
00114
00115 namespace vcg {
00116
00121 template<class S> class Quaternion: public Point4<S> {
00122 public:
00123
00124 Quaternion() {}
00125 Quaternion(const S v0, const S v1, const S v2, const S v3): Point4<S>(v0,v1,v2,v3){}
00126 Quaternion(const Point4<S> p) : Point4<S>(p) {}
00127 Quaternion(const S phi, const Point3<S> &a);
00128
00129 Quaternion operator*(const S &s) const;
00130
00131 Quaternion operator*(const Quaternion &q) const;
00132 Quaternion &operator*=(const Quaternion &q);
00133 void Invert();
00134 Quaternion<S> Inverse() const;
00135
00136
00137 void SetIdentity();
00138
00139 void FromAxis(const S phi, const Point3<S> &a);
00140 void ToAxis(S &phi, Point3<S> &a ) const;
00141
00143 void FromMatrix(const Matrix44<S> &m);
00144 void FromMatrix(const Matrix33<S> &m);
00145
00146 void ToMatrix(Matrix44<S> &m) const;
00147 void ToMatrix(Matrix33<S> &m) const;
00148
00149 void ToEulerAngles(S &alpha, S &beta, S &gamma) const;
00150 void FromEulerAngles(S alpha, S beta, S gamma);
00151
00152 Point3<S> Rotate(const Point3<S> vec) const;
00153
00154
00155
00156 const S & V ( const int i ) const { assert(i>=0 && i<4); return Point4<S>::V(i); }
00157 S & V ( const int i ) { assert(i>=0 && i<4); return Point4<S>::V(i); }
00158
00160 template <class Q>
00161 static inline Quaternion Construct( const Quaternion<Q> & b )
00162 {
00163 return Quaternion(S(b[0]),S(b[1]),S(b[2]),S(b[3]));
00164 }
00165
00166 private:
00167 };
00168
00169
00170
00171
00172 template <class S> Quaternion<S> Interpolate( Quaternion<S> a, Quaternion<S> b, double t);
00173 template <class S> Quaternion<S> &Invert(Quaternion<S> &q);
00174 template <class S> Quaternion<S> Inverse(const Quaternion<S> &q);
00175
00176
00177
00178 template <class S>
00179 void Quaternion<S>::SetIdentity(){
00180 FromAxis(0, Point3<S>(1, 0, 0));
00181 }
00182
00183
00184 template <class S> Quaternion<S>::Quaternion(const S phi, const Point3<S> &a) {
00185 FromAxis(phi, a);
00186 }
00187
00188
00189 template <class S> Quaternion<S> Quaternion<S>::operator*(const S &s) const {
00190 return (Quaternion(V(0)*s,V(1)*s,V(2)*s,V(3)*s));
00191 }
00192
00193 template <class S> Quaternion<S> Quaternion<S>::operator*(const Quaternion &q) const {
00194 Point3<S> t1(V(1), V(2), V(3));
00195 Point3<S> t2(q.V(1), q.V(2), q.V(3));
00196
00197 S d = t2.dot(t1);
00198 Point3<S> t3 = t1 ^ t2;
00199
00200 t1 *= q.V(0);
00201 t2 *= V(0);
00202
00203 Point3<S> tf = t1 + t2 +t3;
00204
00205 Quaternion<S> t;
00206 t.V(0) = V(0) * q.V(0) - d;
00207 t.V(1) = tf[0];
00208 t.V(2) = tf[1];
00209 t.V(3) = tf[2];
00210 return t;
00211 }
00212
00213 template <class S> Quaternion<S> &Quaternion<S>::operator*=(const Quaternion &q) {
00214 S ww = V(0) * q.V(0) - V(1) * q.V(1) - V(2) * q.V(2) - V(3) * q.V(3);
00215 S xx = V(0) * q.V(1) + V(1) * q.V(0) + V(2) * q.V(3) - V(3) * q.V(2);
00216 S yy = V(0) * q.V(2) - V(1) * q.V(3) + V(2) * q.V(0) + V(3) * q.V(1);
00217
00218 V(0) = ww;
00219 V(1) = xx;
00220 V(2) = yy;
00221 V(3) = V(0) * q.V(3) + V(1) * q.V(2) - V(2) * q.V(1) + V(3) * q.V(0);
00222 return *this;
00223 }
00224
00225 template <class S> void Quaternion<S>::Invert() {
00226 V(1)*=-1;
00227 V(2)*=-1;
00228 V(3)*=-1;
00229 }
00230
00231 template <class S> Quaternion<S> Quaternion<S>::Inverse() const{
00232 return Quaternion<S>( V(0), -V(1), -V(2), -V(3) );
00233 }
00234
00235 template <class S> void Quaternion<S>::FromAxis(const S phi, const Point3<S> &a) {
00236 Point3<S> b = a;
00237 b.Normalize();
00238 S s = math::Sin(phi/(S(2.0)));
00239
00240 V(0) = math::Cos(phi/(S(2.0)));
00241 V(1) = b[0]*s;
00242 V(2) = b[1]*s;
00243 V(3) = b[2]*s;
00244 }
00245
00246 template <class S> void Quaternion<S>::ToAxis(S &phi, Point3<S> &a) const {
00247 S s = math::Asin(V(0))*S(2.0);
00248 phi = math::Acos(V(0))*S(2.0);
00249
00250 if(s < 0)
00251 phi = - phi;
00252
00253 a.V(0) = V(1);
00254 a.V(1) = V(2);
00255 a.V(2) = V(3);
00256 a.Normalize();
00257 }
00258
00259
00260 template <class S> Point3<S> Quaternion<S>::Rotate(const Point3<S> p) const {
00261 Quaternion<S> co = *this;
00262 co.Invert();
00263
00264 Quaternion<S> tmp(0, p.V(0), p.V(1), p.V(2));
00265
00266 tmp = (*this) * tmp * co;
00267 return Point3<S>(tmp.V(1), tmp.V(2), tmp.V(3));
00268 }
00269
00270
00271 template<class S, class M> void QuaternionToMatrix(const Quaternion<S> &q, M &m) {
00272 float x2 = q.V(1) + q.V(1);
00273 float y2 = q.V(2) + q.V(2);
00274 float z2 = q.V(3) + q.V(3);
00275 {
00276 float xx2 = q.V(1) * x2;
00277 float yy2 = q.V(2) * y2;
00278 float zz2 = q.V(3) * z2;
00279 m[0][0] = 1.0f - yy2 - zz2;
00280 m[1][1] = 1.0f - xx2 - zz2;
00281 m[2][2] = 1.0f - xx2 - yy2;
00282 }
00283 {
00284 float yz2 = q.V(2) * z2;
00285 float wx2 = q.V(0) * x2;
00286 m[1][2] = yz2 - wx2;
00287 m[2][1] = yz2 + wx2;
00288 }
00289 {
00290 float xy2 = q.V(1) * y2;
00291 float wz2 = q.V(0) * z2;
00292 m[0][1] = xy2 - wz2;
00293 m[1][0] = xy2 + wz2;
00294 }
00295 {
00296 float xz2 = q.V(1) * z2;
00297 float wy2 = q.V(0) * y2;
00298 m[2][0] = xz2 - wy2;
00299 m[0][2] = xz2 + wy2;
00300 }
00301 }
00302
00303 template <class S> void Quaternion<S>::ToMatrix(Matrix44<S> &m) const {
00304 QuaternionToMatrix<S, Matrix44<S> >(*this, m);
00305 m[0][3] = (S)0.0;
00306 m[1][3] = (S)0.0;
00307 m[2][3] = (S)0.0;
00308 m[3][0] = (S)0.0;
00309 m[3][1] = (S)0.0;
00310 m[3][2] = (S)0.0;
00311 m[3][3] = (S)1.0;
00312 }
00313
00314 template <class S> void Quaternion<S>::ToMatrix(Matrix33<S> &m) const {
00315 QuaternionToMatrix<S, Matrix33<S> >(*this, m);
00316
00317
00318 }
00319
00320
00321 template<class S, class M> void MatrixToQuaternion(const M &m, Quaternion<S> &q) {
00322
00323 if ( m[0][0] + m[1][1] + m[2][2] > 0.0f ) {
00324 S t = m[0][0] + m[1][1] + m[2][2] + 1.0f;
00325 S s = (S)0.5 / math::Sqrt(t);
00326 q.V(0) = s * t;
00327 q.V(3) = ( m[1][0] - m[0][1] ) * s;
00328 q.V(2) = ( m[0][2] - m[2][0] ) * s;
00329 q.V(1) = ( m[2][1] - m[1][2] ) * s;
00330 } else if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
00331 S t = m[0][0] - m[1][1] - m[2][2] + 1.0f;
00332 S s = (S)0.5 / math::Sqrt(t);
00333 q.V(1) = s * t;
00334 q.V(2) = ( m[1][0] + m[0][1] ) * s;
00335 q.V(3) = ( m[0][2] + m[2][0] ) * s;
00336 q.V(0) = ( m[2][1] - m[1][2] ) * s;
00337 } else if ( m[1][1] > m[2][2] ) {
00338 S t = - m[0][0] + m[1][1] - m[2][2] + 1.0f;
00339 S s = (S)0.5 / math::Sqrt(t);
00340 q.V(2) = s * t;
00341 q.V(1) = ( m[1][0] + m[0][1] ) * s;
00342 q.V(0) = ( m[0][2] - m[2][0] ) * s;
00343 q.V(3) = ( m[2][1] + m[1][2] ) * s;
00344 } else {
00345 S t = - m[0][0] - m[1][1] + m[2][2] + 1.0f;
00346 S s = (S)0.5 / math::Sqrt(t);
00347 q.V(3) = s * t;
00348 q.V(0) = ( m[1][0] - m[0][1] ) * s;
00349 q.V(1) = ( m[0][2] + m[2][0] ) * s;
00350 q.V(2) = ( m[2][1] + m[1][2] ) * s;
00351 }
00352 }
00353
00354
00355 template <class S> void Quaternion<S>::FromMatrix(const Matrix44<S> &m) {
00356 MatrixToQuaternion<S, Matrix44<S> >(m, *this);
00357 }
00358 template <class S> void Quaternion<S>::FromMatrix(const Matrix33<S> &m) {
00359 MatrixToQuaternion<S, Matrix33<S> >(m, *this);
00360 }
00361
00362
00363 template<class S>
00364 void Quaternion<S>::ToEulerAngles(S &alpha, S &beta, S &gamma) const
00365 {
00366 #define P(a,b,c,d) (2*(V(a)*V(b)+V(c)*V(d)))
00367 #define M(a,b,c,d) (2*(V(a)*V(b)-V(c)*V(d)))
00368 alpha = math::Atan2( P(0,1,2,3) , 1-P(1,1,2,2) );
00369 beta = math::Asin ( M(0,2,3,1) );
00370 gamma = math::Atan2( P(0,3,1,2) , 1-P(2,2,3,3) );
00371 #undef P
00372 #undef M
00373 }
00374
00375 template<class S>
00376 void Quaternion<S>::FromEulerAngles(S alpha, S beta, S gamma)
00377 {
00378 S cosalpha = math::Cos(alpha / 2.0);
00379 S cosbeta = math::Cos(beta / 2.0);
00380 S cosgamma = math::Cos(gamma / 2.0);
00381 S sinalpha = math::Sin(alpha / 2.0);
00382 S sinbeta = math::Sin(beta / 2.0);
00383 S singamma = math::Sin(gamma / 2.0);
00384
00385 V(0) = cosalpha * cosbeta * cosgamma + sinalpha * sinbeta * singamma;
00386 V(1) = sinalpha * cosbeta * cosgamma - cosalpha * sinbeta * singamma;
00387 V(2) = cosalpha * sinbeta * cosgamma + sinalpha * cosbeta * singamma;
00388 V(3) = cosalpha * cosbeta * singamma - sinalpha * sinbeta * cosgamma;
00389 }
00390
00391 template <class S> Quaternion<S> &Invert(Quaternion<S> &m) {
00392 m.Invert();
00393 return m;
00394 }
00395
00396 template <class S> Quaternion<S> Inverse(const Quaternion<S> &m) {
00397 Quaternion<S> a = m;
00398 a.Invert();
00399 return a;
00400 }
00401
00402 template <class S> Quaternion<S> Interpolate( Quaternion<S> a , Quaternion<S> b , double t) {
00403
00404 double v = a.V(0) * b.V(0) + a.V(1) * b.V(1) + a.V(2) * b.V(2) + a.V(3) * b.V(3);
00405 double phi = math::Acos(v);
00406 if(phi > 0.01) {
00407 a = a * (math::Sin(phi *(1-t))/math::Sin(phi));
00408 b = b * (math::Sin(phi * t)/math::Sin(phi));
00409 }
00410
00411 Quaternion<S> c;
00412 c.V(0) = a.V(0) + b.V(0);
00413 c.V(1) = a.V(1) + b.V(1);
00414 c.V(2) = a.V(2) + b.V(2);
00415 c.V(3) = a.V(3) + b.V(3);
00416
00417 if(v < -0.999) {
00418 double d = t * (1 - t);
00419 if(c.V(0) == 0)
00420 c.V(0) += d;
00421 else
00422 c.V(1) += d;
00423 }
00424 c.Normalize();
00425 return c;
00426 }
00427
00428
00429
00430 typedef Quaternion<float> Quaternionf;
00431 typedef Quaternion<double> Quaterniond;
00432
00433 }
00434
00435
00436 #endif
