Subversion Repository Public Repository

Divide-Framework

This repository has no backups
This repository's network speed is throttled to 100KB/sec

Diff Revisions 167 vs 168 for /trunk/Source Code/Core/Math/Headers/Quaternion.h

Diff revisions: vs.
  @@ -30,60 +30,187 @@
30 30 */
31 31 #include "core.h"
32 32
33 - #define TOLERANCE 0.00001f
34 -
35 33 template<class T>
36 34 class Quaternion
37 35 {
38 36 public:
39 37 Quaternion(T x, T y, T z, T w) : _x(x), _y(y), _z(z), _w(w),_dirty(true){}
40 - Quaternion() : _x(0), _y(0), _z(0), _w(0),_dirty(true) {}
41 - Quaternion(const mat3<T>& rotationMatrix) : _dirty(true){FromMatrix(rotationMatrix);}
38 + Quaternion() : _x(0), _y(0), _z(0), _w(1),_dirty(true) {}
39 + Quaternion(const mat3<T>& rotationMatrix) : _dirty(true){fromMatrix(rotationMatrix);}
40 + Quaternion(const vec3<T>& axis, T angle) : _dirty(true) {fromAxisAngle(axis, angle);}
41 + Quaternion(T pitch, T yaw, T roll,bool inDegrees = true) : _dirty(true) {fromEuler(pitch,yaw,roll,inDegrees);}
42 +
43 +
44 + inline T dot(const Quaternion& rq) const { return _w*rq._w+_x*rq._x+_y*rq._y+_z*rq._z; }
45 + inline T magnitude() const { return sqrtf(magnituteSq()); }
46 + inline T magnituteSq() const { return (_w * _w + _x * _x + _y * _y + _z * _z); }
47 +
48 + inline bool compare(const Quaternion& rq, F32 tolerance = 1e-3f) const {
49 + T fCos = dot(rq);
50 + T angle = (T)acos((D32)fCos);
51 + T angleRad = RADIANS(angle);
52 + F32 toleranceRad = RADIANS(tolerance);
53 +
54 + return (abs(angleRad) <= toleranceRad) || FLOAT_COMPARE_TOLERANCE(angleRad, M_PI, toleranceRad);
55 + }
56 +
57 + inline void set(T x, T y, T z, T w) {_x = x; _y = y; _z = z; _w = w; _dirty = true;}
58 +
42 59 //! normalising a quaternion works similar to a vector. This method will not do anything
43 - //! if the quaternion is close enough to being unit-length. define TOLERANCE as something
60 + //! if the quaternion is close enough to being unit-length. define EPSILON as something
44 61 //! small like 0.00001f to get accurate results
45 - void normalize() {
46 - _dirty = true;
62 + inline void normalize() {
47 63 // Don't normalize if we don't have to
48 64 T mag2 = (_w * _w + _x * _x + _y * _y + _z * _z);
49 - if ( mag2!=0.f && (fabs(mag2 - 1.0f) > TOLERANCE)) {
65 + if ( mag2!=0.f && (fabs(mag2 - 1.0f) > EPSILON)) {
50 66 T mag = square_root_tpl(mag2);
51 67 _w /= mag;
52 68 _x /= mag;
53 69 _y /= mag;
54 70 _z /= mag;
71 + _dirty = true;
55 72 }
56 73 }
74 +
75 + inline Quaternion inverse() const {
76 + T invMag = 1.0f / magnitude();
77 + return getConjugate() * invMag;
78 + }
79 +
57 80 //! We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector
58 81 //! The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length
59 82 inline Quaternion getConjugate() const { return Quaternion<T>(-_x, -_y, -_z, _w);}
83 +
60 84 //! Multiplying q1 with q2 applies the rotation q2 to q1
61 85 //! the constructor takes its arguments as (x, y, z, w)
62 - Quaternion operator* (const Quaternion &rq) const {
86 + inline Quaternion operator* (const Quaternion& rq) const {
63 87 return Quaternion<T>(_w * rq._x + _x * rq._w + _y * rq._z - _z * rq._y,
64 88 _w * rq._y + _y * rq._w + _z * rq._x - _x * rq._z,
65 89 _w * rq._z + _z * rq._w + _x * rq._y - _y * rq._x,
66 90 _w * rq._w - _x * rq._x - _y * rq._y - _z * rq._z);
67 91 }
92 +
93 + //! Multiply so that rotations are applied in a left to right order.
94 + inline Quaternion& operator*=(const Quaternion& rq){
95 + Quaternion tmp((_w * rq._w) - (_x * rq._x) - (_y * rq._y) - (_z * rq._z),
96 + (_w * rq._x) + (_x * rq._w) - (_y * rq._z) + (_z * rq._y),
97 + (_w * rq._y) + (_x * rq._z) + (_y * rq._w) - (_z * rq._x),
98 + (_w * rq._z) - (_x * rq._y) + (_y * rq._x) + (_z * rq._w));
99 +
100 + *this = tmp;
101 + _dirty = true;
102 + return *this;
103 + }
104 +
68 105 //! Multiplying a quaternion q with a vector v applies the q-rotation to v
69 - vec3<T> operator* (const vec3<T> &vec) const {
70 - vec3<T> vn(vec);
71 - vn.normalize();
106 + vec3<T> operator* (const vec3<T>& vec) const {
107 + // nVidia SDK implementation
108 + vec3<T> uv, uuv;
109 + vec3<T> qvec(_x, _y, _z);
110 + uv.cross(qvec,vec);
111 + uuv.cross(qvec,uv);
112 + uv *= (2.0f * _w);
113 + uuv *= 2.0f;
114 +
115 + return vec + uv + uuv;
116 + }
117 +
118 + inline bool operator==(const Quaternion& rq) const { return compare(rq); }
119 + inline bool operator!=(const Quaternion& rq) const { return !(*this == rq); }
120 +
121 + inline Quaternion& operator+=(const Quaternion& rq) {
122 + _w += rq._w;
123 + _x += rq._x;
124 + _y += rq._y;
125 + _z += rq._z;
126 + _dirty = true;
127 + return *this;
128 + }
129 +
130 + inline Quaternion& operator-=(const Quaternion& rq) {
131 + _w -= rq._w;
132 + _x -= rq._x;
133 + _y -= rq._y;
134 + _z -= rq._z;
135 + _dirty = true;
136 + return *this;
137 + }
138 +
139 + inline Quaternion& operator*=(T scalar) {
140 + _w *= scalar;
141 + _x *= scalar;
142 + _y *= scalar;
143 + _z *= scalar;
144 + _dirty = true;
145 + return *this;
146 + }
147 +
148 + inline Quaternion& operator/=(T scalar) {
149 + _w /= scalar, _x /= scalar, _y /= scalar, _z /= scalar;
150 + _dirty = true;
151 + return *this;
152 + }
72 153
73 - Quaternion<T> vecQuat, resQuat;
74 - vecQuat._x = vn.x;
75 - vecQuat._y = vn.y;
76 - vecQuat._z = vn.z;
77 - vecQuat._w = 0.0f;
154 + inline Quaternion operator+(const Quaternion& rq) const {
155 + Quaternion tmp(*this);
156 + tmp += rq;
157 + return tmp;
158 + }
159 +
160 + inline Quaternion operator-(const Quaternion& rq) const {
161 + Quaternion tmp(*this);
162 + tmp -= rq;
163 + return tmp;
164 + }
165 +
166 + inline Quaternion operator*(T scalar) const {
167 + Quaternion tmp(*this);
168 + tmp *= scalar;
169 + return tmp;
170 + }
171 +
172 + inline Quaternion operator/(T scalar) const {
173 + Quaternion tmp(*this);
174 + tmp /= scalar;
175 + return tmp;
176 + }
78 177
79 - resQuat = vecQuat * getConjugate();
80 - resQuat = *this * resQuat;
178 + inline void slerp(const Quaternion& q, F32 t) { slerp(this, q1, t); }
81 179
82 - return (vec3<T>(resQuat._x, resQuat._y, resQuat._z));
180 + void slerp(const Quaternion& q0,const Quaternion& q1,F32 t) {
181 + F32 k0,k1,cosomega = q0._x * q1._x + q0._y * q1._y + q0._z * q1._z + q0._w * q1._w;
182 + Quaternion q;
183 + if(cosomega < 0.0) {
184 + cosomega = -cosomega;
185 + q._x = -q1._x;
186 + q._y = -q1._y;
187 + q._z = -q1._z;
188 + q._w = -q1._w;
189 + } else {
190 + q._x = q1._x;
191 + q._y = q1._y;
192 + q._z = q1._z;
193 + q._w = q1._w;
194 + }
195 + if(1.0 - cosomega > 1e-6) {
196 + F32 omega = (F32)acos(cosomega);
197 + F32 sinomega = (F32)sin(omega);
198 + k0 = (F32)sin((1.0f - t) * omega) / sinomega;
199 + k1 = (F32)sin(t * omega) / sinomega;
200 + } else {
201 + k0 = 1.0f - t;
202 + k1 = t;
203 + }
204 + this->_x = q0._x * k0 + q._x * k1;
205 + this->_y = q0._y * k0 + q._y * k1;
206 + this->_z = q0._z * k0 + q._z * k1;
207 + this->_w = q0._w * k0 + q._w * k1;
208 +
209 + _dirty = true;
83 210 }
84 211
85 212 //! Convert from Axis Angle
86 - void FromAxis(const vec3<T>& v, T angle){
213 + void fromAxisAngle(const vec3<T>& v, T angle){
87 214 _dirty = true;
88 215 T sinAngle;
89 216 angle = RADIANS(angle);
  @@ -99,66 +226,80 @@
99 226 _w = cos(angle);
100 227 }
101 228
102 - inline void FromEuler(const vec3<T>& v) {FromEuler(v.x,v.y,v.z);}
229 + inline void fromEuler(const vec3<T>& v, bool inDegrees = true) {
230 + fromEuler(v.pitch,v.yaw,v.roll,inDegrees);
231 + }
103 232
104 233 //! Convert from Euler Angles
105 - //! Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
106 - //! and multiply those together.
107 - //! the calculation below does the same, just shorter
108 - void FromEuler(T pitch, T yaw, T roll) {
109 - _dirty = true;
110 - T M_PIDIV180_2 = M_PIDIV180 / 2.0;
111 - T p = pitch * M_PIDIV180_2;
112 - T y = yaw * M_PIDIV180_2;
113 - T r = roll * M_PIDIV180_2;
114 -
115 - T sinp = sin(p);
116 - T siny = sin(y);
117 - T sinr = sin(r);
118 - T cosp = cos(p);
119 - T cosy = cos(y);
120 - T cosr = cos(r);
121 -
122 - this->_x = sinr * cosp * cosy - cosr * sinp * siny;
123 - this->_y = cosr * sinp * cosy + sinr * cosp * siny;
124 - this->_z = cosr * cosp * siny - sinr * sinp * cosy;
125 - this->_w = cosr * cosp * cosy + sinr * sinp * siny;
126 - normalize();
127 - }
128 -
129 - void FromMatrix(const mat3<T>& rotationMatrix) {
130 - T t = 1 + rotationMatrix.a1 + rotationMatrix.b2 + rotationMatrix.c3;
131 -
132 - // large enough
133 - if( t > static_cast<TReal>(0.001)){
134 - T s = sqrt( t) * static_cast<T>(2.0);
135 - _x = (rotationMatrix.c2 - rotationMatrix.b3) / s;
136 - _y = (rotationMatrix.a3 - rotationMatrix.c1) / s;
137 - _z = (rotationMatrix.b1 - rotationMatrix.a2) / s;
138 - _w = static_cast<T>(0.25) * s;
139 - // else we have to check several cases
140 - }else if( pRotMatrix.a1 > pRotMatrix.b2 && pRotMatrix.a1 > pRotMatrix.c3 ){
141 - // Column 0:
142 - T s = sqrt( static_cast<T>(1.0) + pRotMatrix.a1 - pRotMatrix.b2 - pRotMatrix.c3) * static_cast<T>(2.0);
143 - _x = static_cast<T>(0.25) * s;
144 - _y = (rotationMatrix.b1 + rotationMatrix.a2) / s;
145 - _z = (rotationMatrix.a3 + rotationMatrix.c1) / s;
146 - _w = (rotationMatrix.c2 - rotationMatrix.b3) / s;
147 - }else if( rotationMatrix.b2 > rotationMatrix.c3){
148 - // Column 1:
149 - T s = sqrt( static_cast<T>(1.0) + rotationMatrix.b2 - rotationMatrix.a1 - rotationMatrix.c3) * static_cast<T>(2.0);
150 - _x = (rotationMatrix.b1 + rotationMatrix.a2) / s;
151 - _y = static_cast<T>(0.25) * s;
152 - _z = (rotationMatrix.c2 + rotationMatrix.b3) / s;
153 - _w = (rotationMatrix.a3 - rotationMatrix.c1) / s;
154 - }else{
155 - // Column 2:
156 - T s = sqrt( static_cast<T>(1.0) + pRotMatrix.c3 - pRotMatrix.a1 - pRotMatrix.b2) * static_cast<T>(2.0);
157 - _x = (rotationMatrix.a3 + rotationMatrix.c1) / s;
158 - _y = (rotationMatrix.c2 + rotationMatrix.b3) / s;
159 - _z = static_cast<T>(0.25) * s;
160 - _w = (rotationMatrix.b1 - rotationMatrix.a2) / s;
234 + void fromEuler(T pitch, T yaw, T roll, bool inDegrees = true) {
235 +
236 + _dirty = true;
237 +
238 + T attitude = pitch;
239 + T heading = yaw;
240 + T bank = roll;
241 +
242 + if(inDegrees){
243 + attitude = RADIANS(attitude);
244 + heading = RADIANS(heading);
245 + bank = RADIANS(bank);
161 246 }
247 +
248 + D32 c1 = cos(heading * 0.5);
249 + D32 s1 = sin(heading * 0.5);
250 + D32 c2 = cos(attitude * 0.5);
251 + D32 s2 = sin(attitude * 0.5);
252 + D32 c3 = cos(bank * 0.5);
253 + D32 s3 = sin(bank * 0.5);
254 +
255 + D32 c1c2 = c1*c2;
256 + D32 s1s2 = s1*s2;
257 +
258 + this->_w = (T)(c1c2 * c3 - s1s2 * s3);
259 + this->_x = (T)(c1c2 * s3 + s1s2 * c3);
260 + this->_y = (T)(s1 * c2 * c3 + c1 * s2 * s3);
261 + this->_z = (T)(c1 * s2 * c3 - s1 * c2 * s3);
262 +
263 + //normalize(); this method does produce a normalized quaternion
264 + }
265 +
266 + //a la Ogre3D
267 + void fromMatrix(const mat3<T>& rotationMatrix) {
268 + _dirty = true;
269 +
270 + // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
271 + // article "Quaternion Calculus and Fast Animation".
272 +
273 + T fTrace = rotationMatrix.m[0][0]+rotationMatrix.m[1][1]+rotationMatrix.m[2][2];
274 + T fRoot;
275 +
276 + if ( fTrace > 0.0 ){
277 + // |w| > 1/2, may as well choose w > 1/2
278 + fRoot = (T)sqrt((F32)fTrace + 1.0f); // 2w
279 + _w = 0.5f*fRoot;
280 + fRoot = 0.5f/fRoot; // 1/(4w)
281 + _x = (rotationMatrix.m[2][1]-rotationMatrix.m[1][2])*fRoot;
282 + _y = (rotationMatrix.m[0][2]-rotationMatrix.m[2][0])*fRoot;
283 + _z = (rotationMatrix.m[1][0]-rotationMatrix.m[0][1])*fRoot;
284 + }else{
285 + // |w| <= 1/2
286 + static size_t s_iNext[3] = { 1, 2, 0 };
287 + size_t i = 0;
288 + if ( rotationMatrix.m[1][1] > rotationMatrix.m[0][0] )
289 + i = 1;
290 + if ( rotationMatrix.m[2][2] > rotationMatrix.m[i][i] )
291 + i = 2;
292 + size_t j = s_iNext[i];
293 + size_t k = s_iNext[j];
294 +
295 + fRoot = (T)sqrt((F32)(rotationMatrix.m[i][i]-rotationMatrix.m[j][j]-rotationMatrix.m[k][k] + 1.0f));
296 + T* apkQuat[3] = { &_x, &_y, &_z };
297 + *apkQuat[i] = 0.5f*fRoot;
298 + fRoot = 0.5f/fRoot;
299 + _w = (rotationMatrix.m[k][j]-rotationMatrix.m[j][k])*fRoot;
300 + *apkQuat[j] = (rotationMatrix.m[j][i]+rotationMatrix.m[i][j])*fRoot;
301 + *apkQuat[k] = (rotationMatrix.m[k][i]+rotationMatrix.m[i][k])*fRoot;
302 + }
162 303 }
163 304
164 305 //! Convert to Matrix
  @@ -188,28 +329,59 @@
188 329 }
189 330
190 331 //! Convert to Axis/Angles
191 - void getAxisAngle(vec3<T> *axis, T *angle,bool inDegrees) const{
332 + void getAxisAngle(vec3<T> *axis, T *angle,bool inDegrees) const {
192 333 T scale = square_root_tpl(_x * _x + _y * _y + _z * _z);
193 334 axis->x = _x / scale;
194 335 axis->y = _y / scale;
195 336 axis->z = _z / scale;
196 - if(inDegrees)
197 - *angle = DEGREES(acos(_w) * 2.0f);
198 - else
199 - *angle = acos(_w) * 2.0f;
337 + if(inDegrees) *angle = DEGREES(acos(_w) * 2.0f);
338 + else *angle = acos(_w) * 2.0f;
200 339 }
201 340
202 - inline bool compare(const Quaternion& q) const {
203 - vec4<T> thisQ(_x,_y,_z,_w);
204 - vec4<T> otherQ(q._x,q._y,q._z,q._w);
341 + void getEuler(vec3<T> *euler, bool toDegrees = false) const {
342 + T heading = 0, attitude = 0, bank = 0;
343 +
344 + T sqx = _x * _x;
345 + T sqy = _y * _y;
346 + T sqz = _z * _z;
347 + T sqw = _w * _w;
348 + T test = _x * _y + _z * _w;
349 + T unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
350 +
351 + if(test > (0.5f-EPSILON) * unit) { // singularity at north pole
352 + heading = 2 * atan2(_x , _w);
353 + attitude = M_PIDIV2;
354 + bank = 0;
355 + }else if (test < -(0.5f - EPSILON) * unit) { // singularity at south pole
356 + heading = -2 * atan2(_x , _w);
357 + attitude = -M_PIDIV2;
358 + bank = 0;
359 + }else{
360 + T x2 = 2 * _x;
361 + T y2 = 2 * _y;
205 362
206 - return thisQ.compare(otherQ);
363 + heading = atan2(y2 * _w - x2 * _z , sqx - sqy - sqz + sqw);
364 + attitude = asin(2 * test / unit);
365 + bank = atan2(x2 * _w - y2 * _z ,-sqx + sqy - sqz + sqw);
366 + }
367 + //Convert back from Z = pitch to Z = roll
368 + if(toDegrees){
369 + euler->yaw = DEGREES(heading);
370 + euler->pitch = DEGREES(bank);
371 + euler->roll = DEGREES(attitude);
372 + }else{
373 + euler->yaw = heading;
374 + euler->pitch = bank;
375 + euler->roll = attitude;
376 + }
207 377 }
208 378
209 - inline T const& getX() const {return _x;}
210 - inline T const& getY() const {return _y;}
211 - inline T const& getZ() const {return _z;}
212 - inline T const& getW() const {return _w;}
379 + inline void identity() { _w = 1, _x = _y = _z = 0; _dirty = true;}
380 + inline const T& getX() const {return _x;}
381 + inline const T& getY() const {return _y;}
382 + inline const T& getZ() const {return _z;}
383 + inline const T& getW() const {return _w;}
384 + inline vec4<T> asVec4() const {return vec4<T>(_x,_y,_z,_w);}
213 385
214 386 private:
215 387 T _x,_y,_z,_w;