00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef RTC_QUATERNION_H
00020 #define RTC_QUATERNION_H
00021
00022
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcEulerAngles.h"
00025 #include "rtc/rtcRotation.h"
00026 #include "rtc/rtcTransform.h"
00027 #include "rtc/rtcSMat3.h"
00028 #include "rtc/rtcSMat4.h"
00029 #include "rtc/rtcVec3.h"
00030 #include "rtc/rtcVec4.h"
00031
00032
00033 namespace rtc {
00034
00035
00036 template <class T> class EulerAngles;
00037 template <class T> class Rotation;
00038 template <class T> class Transform;
00039 template <class T> class Quaternion;
00040 template <class T> class SMat3;
00041 template <class T> class SMat4;
00042 template <class T> class Vec3;
00043
00052 template <class T>
00053 class Quaternion: public Vec4<T> {
00054 public:
00055
00056 using Vec4<T>::x;
00057 using Vec4<T>::set;
00058 using Vec4<T>::normalize;
00059 using Vec4<T>::operator *=;
00060 using Vec4<T>::operator *;
00061
00063 Quaternion();
00064 Quaternion(const T w, const T x, const T y, const T z);
00065 Quaternion(const Vec<T,4>& v);
00066
00067 Quaternion(const Vec<T,3>& n, const T theta);
00068 Quaternion(const EulerAngles<T>& e);
00069 Quaternion(const Rotation<T>& r);
00070
00072 void set(const Vec<T,3>& n, const T theta);
00073 void set(const EulerAngles<T>& e);
00074 void set(const Rotation<T>& r);
00075
00077 T angle() const;
00078 T w() const;
00079 Vec<T,3> axis() const;
00080
00082 Quaternion<T> operator * (const Quaternion<T>& q) const;
00083 void operator *= (const Quaternion<T>& q);
00084 Quaternion<T> conjugated() const;
00085 void conjugate();
00086 Quaternion<T> inverted() const;
00087 void invert();
00088 T angle(const Quaternion<T>& q) const;
00089 Quaternion<T> slerp(const Quaternion<T>& q, const T& t) const;
00090
00092 SMat3<T> drot_dqj(const int j) const;
00093 static SMat3<T> d2rot_dqkdqj(int k, int j);
00094 static SMat4<T> dquatMat_dqj(const int j);
00095 SMat4<T> quatMat();
00096 SMat4<T> quatMatT();
00097 SMat4<T> quatMatBar();
00098 SMat4<T> quatMatBarT();
00099
00101 void rotate(Vec<T,3>& v) const;
00102 Vec<T,3> rotated(const Vec<T,3>& v) const;
00103 };
00104
00105
00106 typedef Quaternion<float> Quaternionf;
00107 typedef Quaternion<double> Quaterniond;
00108
00109
00110
00111
00112
00113
00114
00117 template <class T>
00118 inline Quaternion<T>::Quaternion() {
00119 set(T(1),T(0),T(0),T(0));
00120 }
00121
00124 template <class T>
00125 inline Quaternion<T>::Quaternion(const T w, const T _x, const T y, const T z) {
00126 set(w,_x,y,z);
00127 }
00128
00131 template <class T>
00132 inline Quaternion<T>::Quaternion(const Vec<T,4>& v) : Vec4<T>(v) {}
00133
00138 template <class T>
00139 inline Quaternion<T>::Quaternion(const Vec<T,3>& n, const T theta) {
00140 set(n,theta);
00141 }
00142
00146 template <class T>
00147 inline Quaternion<T>::Quaternion(const EulerAngles<T>& e) {
00148 set(e);
00149 }
00150
00155 template <class T>
00156 inline Quaternion<T>::Quaternion(const Rotation<T>& r) {
00157 set(r);
00158 }
00159
00160
00161
00166 template <class T>
00167 inline void Quaternion<T>::set(const Vec<T,3>& n, const T theta) {
00168 T s = sin(theta/T(2));
00169 Vec<T,3> nn = n.normalized();
00170 set(cos(theta/T(2)),s*nn.x[0],s*nn.x[1],s*nn.x[2]);
00171 normalize();
00172 }
00173
00177 template <class T>
00178 inline void Quaternion<T>::set(const EulerAngles<T>& e) {
00179 using namespace std;
00180 T cr = cos(T(0.5)*e.x[0]); T sr = sin(T(0.5)*e.x[0]);
00181 T cp = cos(T(0.5)*e.x[1]); T sp = sin(T(0.5)*e.x[1]);
00182 T cy = cos(T(0.5)*e.x[2]); T sy = sin(T(0.5)*e.x[2]);
00183 T w = cy*cp*cr + sy*sp*sr;
00184 if (w<0) set(-w,sy*sp*cr-cy*cp*sr,-cy*sp*cr-sy*cp*sr,cy*sp*sr-sy*cp*cr);
00185 else set(w,cy*cp*sr-sy*sp*cr,cy*sp*cr+sy*cp*sr,sy*cp*cr-cy*sp*sr);
00186 }
00187
00192 template <class T>
00193 inline void Quaternion<T>::set(const Rotation<T>& r) {
00194 T n4;
00195 T tr = r.trace();
00196 if (tr > T(0)) {
00197 set(tr + T(1), -(r(1,2) - r(2,1)),
00198 -(r(2,0) - r(0,2)), -(r(0,1) - r(1,0)));
00199 n4 = x[0];
00200 } else if ((r(0,0) > r(1,1)) && (r(0,0) > r(2,2))) {
00201 set(r(1,2) - r(2,1), -(1.0f + r(0,0) - r(1,1) - r(2,2)),
00202 -(r(1,0) + r(0,1)), -(r(2,0) + r(0,2)));
00203 n4 = -x[1];
00204 } else if (r(1,1) > r(2,2)) {
00205 set(r(2,0) - r(0,2), -(r(1,0) + r(0,1)),
00206 -(1.0f + r(1,1) - r(0,0) - r(2,2)), -(r(2,1) + r(1,2)));
00207 n4 = -x[2];
00208 } else {
00209 set(r(0,1) - r(1,0), -(r(2,0) + r(0,2)), -(r(2,1) + r(1,2)),
00210 -(1.0f + r(2,2) - r(0,0) - r(1,1)));
00211 n4 = -x[3];
00212 }
00213 if (x[0] < T(0)) operator *= (T(-0.5)/T(sqrt(n4)));
00214 else operator *= (T(0.5)/T(sqrt(n4)));
00215 }
00216
00222 template <class T>
00223 Quaternion<T> Quaternion<T>::slerp(const Quaternion<T>& q, const T& t) const
00224 {
00225 T theta = angle(q);
00226 if (theta != T(0.0))
00227 {
00228 T d = T(1.0) / rtc_sin(theta);
00229 T s0 = rtc_sin((T(1.0) - t) * theta);
00230 T s1 = rtc_sin(t * theta);
00231 if (dot(q) < 0)
00232 return Quaternion<T>((x[0] * s0 + -q.x[0] * s1) * d,
00233 (x[1] * s0 + -q.x[1] * s1) * d,
00234 (x[2] * s0 + -q.x[2] * s1) * d,
00235 (x[3] * s0 + -q.x[3] * s1) * d);
00236 else
00237 return Quaternion<T>((x[0] * s0 + q.x[0] * s1) * d,
00238 (x[1] * s0 + q.x[1] * s1) * d,
00239 (x[2] * s0 + q.x[2] * s1) * d,
00240 (x[3] * s0 + q.x[3] * s1) * d);
00241
00242 }
00243 else
00244 {
00245 return *this;
00246 }
00247 }
00248
00249
00250
00253 template <class T>
00254 inline T Quaternion<T>::w() const {
00255 return x[0];
00256 }
00257
00260 template <class T>
00261 inline T Quaternion<T>::angle() const {
00262 return T(2.0)*acos(x[0]);
00263 }
00264
00267 template <class T>
00268 inline Vec<T,3> Quaternion<T>::axis() const {
00269 Vec3<T> n(x[1],x[2],x[3]);
00270 n.normalize();
00271 return n;
00272 }
00273
00274
00275
00278 template <class T>
00279 inline Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& q) const {
00280 return Quaternion<T>(x[0]*q.x[0] - x[1]*q.x[1] - x[2]*q.x[2] - x[3]*q.x[3],
00281 x[0]*q.x[1] + x[1]*q.x[0] + x[2]*q.x[3] - x[3]*q.x[2],
00282 x[0]*q.x[2] - x[1]*q.x[3] + x[2]*q.x[0] + x[3]*q.x[1],
00283 x[0]*q.x[3] + x[1]*q.x[2] - x[2]*q.x[1] + x[3]*q.x[0]);
00284 }
00285
00286
00289 template <class T>
00290 inline void Quaternion<T>::operator *= (const Quaternion<T>& q) {
00291 set(x[0]*q.x[0] - x[1]*q.x[1] - x[2]*q.x[2] - x[3]*q.x[3],
00292 x[0]*q.x[1] + x[1]*q.x[0] + x[2]*q.x[3] - x[3]*q.x[2],
00293 x[0]*q.x[2] - x[1]*q.x[3] + x[2]*q.x[0] + x[3]*q.x[1],
00294 x[0]*q.x[3] + x[1]*q.x[2] - x[2]*q.x[1] + x[3]*q.x[0]);
00295 }
00296
00300 template <class T>
00301 inline Quaternion<T> Quaternion<T>::conjugated() const {
00302 return Quaternion<T>(x[0],-x[1],-x[2],-x[3]);
00303 }
00304
00308 template <class T>
00309 inline void Quaternion<T>::conjugate() {
00310 x[1] = -x[1]; x[2] = -x[2]; x[3] = -x[3];
00311 }
00312
00316 template <class T>
00317 inline Quaternion<T> Quaternion<T>::inverted() const {
00318 T l2 = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
00319 return Quaternion<T>(x[0]/l2,-x[1]/l2,-x[2]/l2,-x[3]/l2);
00320 }
00321
00325 template <class T>
00326 inline void Quaternion<T>::invert() {
00327 T l2 = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
00328 x[0] = x[0]/l2; x[1] = -x[1]/l2; x[2] = -x[2]/l2; x[3] = -x[3]/l2;
00329 }
00330
00333 template <class T>
00334 inline T Quaternion<T>::angle(const Quaternion<T>& q) const
00335 {
00336 T s = rtc_sqrt(this->normSqr() * q.normSqr());
00337 if(s == T(0)) return 0.0;
00338 T retval;
00339 if (dot(q) < 0)
00340 retval = rtc_acos(rtc_clamp(dot(-q) / s, (T)-1.0, (T)1.0));
00341
00342 else
00343 retval = rtc_acos(rtc_clamp(dot(q) / s, (T)-1.0, (T)1.0));
00344
00345 return retval;
00346 }
00347
00348
00349
00353 template <class T>
00354 inline SMat3<T> Quaternion<T>::drot_dqj(const int j) const {
00355 switch(j) {
00356 case 0: return SMat3<T>(2*x[0], -2*x[3], 2*x[2],
00357 2*x[3], 2*x[0], -2*x[1],
00358 -2*x[2], 2*x[1], 2*x[0]);
00359 case 1: return SMat3<T>( 2*x[1], 2*x[2], 2*x[3],
00360 2*x[2], -2*x[1], -2*x[0],
00361 2*x[3], 2*x[0], -2*x[1]);
00362 case 2: return SMat3<T>(-2*x[2], 2*x[1], 2*x[0],
00363 2*x[1], 2*x[2], 2*x[3],
00364 -2*x[0], 2*x[3], -2*x[2]);
00365 case 3: return SMat3<T>(-2*x[3], -2*x[0], 2*x[1],
00366 2*x[0], -2*x[3], 2*x[2],
00367 2*x[1], 2*x[2], 2*x[3]);
00368 }
00369 return SMat3<T>(T(0));
00370 }
00371
00375 template <class T>
00376 inline SMat3<T> Quaternion<T>::d2rot_dqkdqj(int k, int j) {
00377 if (j>k) { int temp = j; j=k; k=temp; }
00378 if (j==0 && k==0) return SMat3<T>(1,0,0, 0,1,0, 0,0,1);
00379 if (j==0 && k==1) return SMat3<T>(0,0,0, 0,0,-1, 0,1,0);
00380 if (j==0 && k==2) return SMat3<T>(0,0,1, 0,0,0, -1,0,0);
00381 if (j==0 && k==3) return SMat3<T>(0,-1,0, 1,0,0, 0,0,0);
00382
00383 if (j==1 && k==1) return SMat3<T>(1,0,0, 0,-1,0, 0,0,-1);
00384 if (j==1 && k==2) return SMat3<T>(0,1,0, 1,0,0, 0,0,0);
00385 if (j==1 && k==3) return SMat3<T>(0,0,1, 0,0,0, 1,0,0);
00386
00387 if (j==2 && k==2) return SMat3<T>(-1,0,0, 0,1,0, 0,0,-1);
00388 if (j==2 && k==3) return SMat3<T>(0,0,0, 0,0,1, 0,1,0);
00389
00390 if (j==3 && k==3) return SMat3<T>(-1,0,0, 0,-1,0, 0,0,1);
00391
00392 return SMat3<T>(T(0));
00393 }
00394
00398 template <class T>
00399 inline SMat4<T> Quaternion<T>::quatMat() {
00400 return SMat4<T>(x[0], -x[1], -x[2], -x[3],
00401 x[1], x[0], -x[3], x[2],
00402 x[2], x[3], x[0], -x[1],
00403 x[3], -x[2], x[1], x[0]);
00404 }
00405
00408 template <class T>
00409 inline SMat4<T> Quaternion<T>::quatMatT() {
00410 return SMat4<T>(x[0], x[1], x[2], x[3],
00411 -x[1], x[0], x[3], -x[2],
00412 -x[2], -x[3], x[0], x[1],
00413 -x[3], x[2], -x[1], x[0]);
00414 }
00415
00418 template <class T>
00419 inline SMat4<T> Quaternion<T>::quatMatBar() {
00420 return SMat4<T>(x[0], -x[1], -x[2], -x[3],
00421 x[1], x[0], x[3], -x[2],
00422 x[2], -x[3], x[0], x[1],
00423 x[3], x[2], -x[1], x[0]);
00424 }
00425
00428 template <class T>
00429 inline SMat4<T> Quaternion<T>::quatMatBarT() {
00430 return SMat4<T>(x[0], x[1], x[2], x[3],
00431 -x[1], x[0], -x[3], x[2],
00432 -x[2], x[3], x[0], -x[1],
00433 -x[3], -x[2], x[1], x[0]);
00434 }
00435
00438 template <class T>
00439 inline SMat4<T> Quaternion<T>::dquatMat_dqj(const int j) {
00440 switch(j) {
00441 case 0: return SMat4<T>(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
00442 case 1: return SMat4<T>(0,-1,0,0,1,0,0,0,0,0,0,-1,0,0,1,0);
00443 case 2: return SMat4<T>(0,0,-1,0,0,0,0,1,1,0,0,0,0,-1,0,0);
00444 case 4: return SMat4<T>(0,0,0,-1,0,0,-1,0,0,1,0,0,1,0,0,0);
00445 }
00446 return SMat4<T>(T(0));
00447 }
00448
00451 template <class T>
00452 inline void Quaternion<T>::rotate(Vec<T,3>& v) const {
00453 T a = T(2)*x[0]*x[0] - T(1);
00454 T b = T(2)*(x[1]*v.x[0] + x[2]*v.x[1] + x[3]*v.x[2]);
00455 T u0 = a*v.x[0] + b*x[1] + T(2)*x[0]*(x[2]*v.x[2] - x[3]*v.x[1]);
00456 T u1 = a*v.x[1] + b*x[2] + T(2)*x[0]*(x[3]*v.x[0] - x[1]*v.x[2]);
00457 T u2 = a*v.x[2] + b*x[3] + T(2)*x[0]*(x[1]*v.x[1] - x[2]*v.x[0]);
00458 v.x[0] = u0; v.x[1] = u1; v.x[2] = u2;
00459 }
00460
00463 template <class T>
00464 inline Vec<T,3> Quaternion<T>::rotated(const Vec<T,3>& v) const {
00465 T a = T(2)*x[0]*x[0] - T(1);
00466 T b = T(2)*(x[1]*v.x[0] + x[2]*v.x[1] + x[3]*v.x[2]);
00467 return Vec3<T>(a*v.x[0] + b*x[1] + 2*x[0]*(x[2]*v.x[2] - x[3]*v.x[1]),
00468 a*v.x[1] + b*x[2] + 2*x[0]*(x[3]*v.x[0] - x[1]*v.x[2]),
00469 a*v.x[2] + b*x[3] + 2*x[0]*(x[1]*v.x[1] - x[2]*v.x[0]));
00470 }
00471
00472
00473 }
00474
00475 #endif // RTC_QUATERNION_H defined
00476
