ESyS-Particle
4.0.1
|
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