ESyS-Particle  4.0.1
Quaternion.hpp
00001 
00002 //                                                         //
00003 // Copyright (c) 2003-2011 by The University of Queensland //
00004 // Earth Systems Science Computational Centre (ESSCC)      //
00005 // http://www.uq.edu.au/esscc                              //
00006 //                                                         //
00007 // Primary Business: Brisbane, Queensland, Australia       //
00008 // Licensed under the Open Software License version 3.0    //
00009 // http://www.opensource.org/licenses/osl-3.0.php          //
00010 //                                                         //
00012 
00013 
00014 #ifndef _QUATERNION_HPP
00015 #define _QUATERNION_HPP
00016 //
00017 // ============================================================
00018 //
00019 //                      Quaternion.hpp
00020 
00021 #include "Foundation/Quaternion.h"
00022 #include "Foundation/vec3.h"
00023 #include "Foundation/Matrix3.h"
00024 
00025 //
00026 // ============================================================
00027 //
00028 //             CONSTRUCTORS, DESTRUCTORS
00029 //
00030 // ============================================================
00031 //
00032 
00033 QUATERNION_INLINE Quaternion::Quaternion()
00034   : vector(Vec3::ZERO),
00035     scalar(1.0)
00036 {
00037 }
00038 
00039 QUATERNION_INLINE Quaternion::Quaternion(double d, const Vec3 &v)
00040   : vector(v),
00041     scalar(d)
00042 {
00043 }
00044 
00045 QUATERNION_INLINE Quaternion::Quaternion(const Quaternion & q)
00046   : vector(q.vector),
00047     scalar(q.scalar)
00048 {
00049 }
00050 
00051 //
00052 // ============================================================
00053 //
00054 //            ASSIGNMENT
00055 //
00056 // ============================================================
00057 //
00058 
00059 QUATERNION_INLINE Quaternion& Quaternion::operator=(const Quaternion& q)
00060 {
00061 #if 0
00062   if (&q == this) return *this;
00063 #endif
00064   vector = q.vector;
00065   scalar = q.scalar;
00066 
00067   return *this;
00068 }
00069 
00070 //
00071 // ============================================================
00072 //
00073 //            OUTPUT
00074 //
00075 // ============================================================
00076 //
00077 
00078 QUATERNION_INLINE std::ostream& operator<<(std::ostream& co, const Quaternion& q)
00079 {
00080   return q.output(co);
00081 }
00082 
00083 QUATERNION_INLINE std::istream& operator>>(std::istream& ci, Quaternion& q)
00084 {
00085   return q.input(ci);
00086 }
00087 
00088 QUATERNION_INLINE std::ostream& Quaternion::output(std::ostream& co) const 
00089 {
00090   co
00091      << scalar << ' '
00092      << vector;
00093 
00094   return co;
00095 }
00096 
00097 QUATERNION_INLINE std::istream& Quaternion::input(std::istream& ci)
00098 {
00099   ci
00100      >> scalar
00101      >> vector;
00102 
00103   return ci;
00104 }
00105 
00106 QUATERNION_INLINE bool Quaternion::operator==(const Quaternion &q) const
00107 {
00108   return
00109     (
00110       (return_sca() == q.return_sca())
00111       &&
00112       (return_vec() == q.return_vec())
00113     );
00114 }
00115 
00116 QUATERNION_INLINE bool Quaternion::operator!=(const Quaternion &q) const
00117 {
00118   return !(*this == q);
00119 }
00120 
00121 //
00122 // ============================================================
00123 //
00124 //            ARITHMETIC OPERATIONS
00125 //
00126 // ============================================================
00127 //
00128 
00129 QUATERNION_INLINE Quaternion Quaternion::operator+(const Quaternion& q2) const
00130 {
00131 #if 0
00132   Quaternion qq;
00133 
00134   qq.scalar = scalar + q2.scalar;
00135   qq.vector = vector + q2.vector;
00136 #endif
00137   return Quaternion(scalar + q2.scalar, vector + q2.vector);
00138 }
00139 
00140 QUATERNION_INLINE Quaternion Quaternion::operator-(const Quaternion& q2) const
00141 {
00142 #if 0
00143   Quaternion qq;
00144 
00145   qq.scalar = scalar - q2.scalar;
00146   qq.vector = vector - q2.vector;
00147 #endif
00148   return Quaternion(scalar-q2.scalar, vector-q2.vector);
00149 }
00150 
00151 QUATERNION_INLINE Quaternion Quaternion::operator-() const
00152 {
00153 #if 0
00154   Quaternion qq;
00155 
00156   qq.scalar = -scalar;
00157   qq.vector = -vector;
00158 #endif
00159   return Quaternion(-scalar, -vector);
00160 }
00161 
00162 QUATERNION_INLINE Quaternion operator*(double c, const Quaternion& q) 
00163 {
00164 #if 0
00165   Quaternion qq;
00166 
00167   qq.scalar = c * q.scalar;
00168   qq.vector = c * q.vector;
00169 #endif
00170   return Quaternion(c*q.scalar, c*q.vector);
00171 }
00172 
00173 QUATERNION_INLINE Quaternion Quaternion::operator*(double c) const
00174 {
00175   return Quaternion(c * scalar, c * vector);
00176 }
00177 
00178 QUATERNION_INLINE Quaternion Quaternion::operator*(const Quaternion& q2) const
00179 {
00180 #if 0
00181   Quaternion qq;
00182 
00183   qq.scalar = scalar * q2.scalar - dot(vector, q2.vector);
00184   qq.vector = scalar * q2.vector
00185              + q2.scalar * vector
00186              + cross(vector, q2.vector);
00187 #endif
00188   return 
00189     Quaternion(
00190       scalar * q2.scalar - dot(vector, q2.vector),
00191       scalar * q2.vector
00192       + q2.scalar * vector
00193       + cross(vector, q2.vector)
00194     );
00195 }
00196 
00197 QUATERNION_INLINE Quaternion Quaternion::inverse() const
00198 {
00199   return Quaternion(scalar, -vector);
00200 }
00201 
00202 QUATERNION_INLINE Quaternion Quaternion::operator/(const Quaternion& q2) const
00203 {
00204   return *this * q2.inverse();
00205 }
00206 
00207 QUATERNION_INLINE Quaternion& Quaternion::operator+=(const Quaternion& q)
00208 {
00209   scalar += q.scalar;
00210   vector += q.vector;
00211 
00212   return *this;
00213 }
00214 
00215 QUATERNION_INLINE Quaternion& Quaternion::operator-=(const Quaternion& q)
00216 {
00217   scalar -= q.scalar;
00218   vector -= q.vector;
00219 
00220   return *this;
00221 }
00222 
00223 QUATERNION_INLINE Quaternion& Quaternion::operator*=(double c)
00224 {
00225   scalar *= c;
00226   vector *= c;
00227 
00228   return *this;
00229 }
00230 
00231 QUATERNION_INLINE Quaternion& Quaternion::operator*=(const Quaternion& q)
00232 {
00233   const double s = scalar * q.scalar - dot(vector, q.vector);
00234   vector = scalar * q.vector
00235           + q.scalar * vector
00236           + cross(vector, q.vector);
00237   scalar = s;
00238 
00239   return *this;
00240 }
00241 
00242 QUATERNION_INLINE Quaternion& Quaternion::operator/=(const Quaternion& q)
00243 {
00244   Quaternion qq = q.inverse();
00245 
00246   const double s = scalar * qq.scalar - dot(vector, qq.vector);
00247   vector = scalar * qq.vector
00248           + qq.scalar * vector
00249           + cross(vector, qq.vector);
00250   scalar = s;
00251 
00252   return *this;
00253 }
00254 
00255 
00256 QUATERNION_INLINE void Quaternion::normalize() 
00257 {
00258   double len = length();
00259 
00260   if (len > 1.0e-8)
00261   {
00262     scalar /= len;
00263     vector /= len;
00264   }
00265 }
00266 
00267 QUATERNION_INLINE double Quaternion::length() const 
00268 {
00269   const double vlen = vector.norm();
00270   return sqrt(scalar * scalar + vlen * vlen);
00271 }
00272 
00273 QUATERNION_INLINE Matrix3 Quaternion::to_matrix() const
00274 {
00275  double m[3][3];
00276 
00277   m[0][0] =   scalar*scalar + vector.X()*vector.X()
00278             - vector.Y()*vector.Y() -vector.Z()*vector.Z();
00279   m[0][1] =   2*(vector.X()*vector.Y() + scalar*vector.Z());
00280   m[0][2] =   2*(vector.X()*vector.Z() - scalar*vector.Y());
00281   m[1][0] =   2*(vector.X()*vector.Y() - scalar*vector.Z());
00282   m[1][1] =   scalar*scalar - vector.X()*vector.X()
00283             + vector.Y()*vector.Y() - vector.Z()*vector.Z();
00284   m[1][2] =   2*(vector.Y()*vector.Z() + scalar*vector.X());
00285   m[2][0] =   2*(vector.X()*vector.Z() + scalar*vector.Y());
00286   m[2][1] =   2*(vector.Y()*vector.Z() - scalar*vector.X());
00287   m[2][2] =   scalar*scalar - vector.X()*vector.X()
00288             - vector.Y()*vector.Y() + vector.Z()*vector.Z();
00289  // In 2D case m[2][2] is not so accurate!
00290 //    m[2][2] = 0.0 ;
00291 
00292 /*
00293 
00294   m[0][0] =  1.0 
00295             - 2.0*vector.Y()*vector.Y() -2.0*vector.Z()*vector.Z();
00296   m[0][1] =   2*(vector.X()*vector.Y() + scalar*vector.Z());
00297   m[0][2] =   2*(vector.X()*vector.Z() - scalar*vector.Y());
00298   m[1][0] =   2*(vector.X()*vector.Y() - scalar*vector.Z());
00299   m[1][1] =  1.0  -2.0* vector.X()*vector.X()
00300              - 2.0*vector.Z()*vector.Z();
00301   m[1][2] =   2*(vector.Y()*vector.Z() + scalar*vector.X());
00302   m[2][0] =   2*(vector.X()*vector.Z() + scalar*vector.Y());
00303   m[2][1] =   2*(vector.Y()*vector.Z() - scalar*vector.X());
00304   m[2][2] =   1.0 - 2.0*vector.X()*vector.X()
00305             - 2.0*vector.Y()*vector.Y() ;
00306 */
00307 
00308   return  Matrix3(m);
00309 }
00310 
00311 QUATERNION_INLINE Vec3 Quaternion::asAngleAxis() const
00312 {
00313   Vec3 v(this->vector);
00314   return (v *= (2*acos(this->scalar)/v.norm()));
00315 }
00316 
00317 QUATERNION_INLINE Quaternion::AngleAxisPair Quaternion::asAngleAxisPair() const
00318 {
00319   return AngleAxisPair(2*acos(this->scalar), this->vector);
00320 }
00321 
00322 #endif // _QUATERNION_HPP