1 //****************************************************************************//
2 // quaternion.cpp //
3 // Copyright (C) 2001, 2002 Bruno 'Beosil' Heidelberger //
4 //****************************************************************************//
5 // This library is free software; you can redistribute it and/or modify it //
6 // under the terms of the GNU Lesser General Public License as published by //
7 // the Free Software Foundation; either version 2.1 of the License, or (at //
8 // your option) any later version. //
9 //****************************************************************************//
10
11 #ifdef HAVE_CONFIG_H
12 #include "config.h"
13 #endif
14
15 //****************************************************************************//
16 // Includes //
17 //****************************************************************************//
18
19 #include "cal3d/quaternion.h"
20 #include "cal3d/matrix.h"
21 #include "cal3d/vector.h"
22
23 #include <float.h>
24
25 /*****************************************************************************/
26 /** Constructs the quaternion instance.
27 *
28 * This function is the default constructor of the quaternion instance.
29 *****************************************************************************/
30 /*
31 CalQuaternion::CalQuaternion()
32 : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
33 {
34 }
35 */
36 /*****************************************************************************/
37 /** Constructs the quaternion instance.
38 *
39 * This function is a constructor of the quaternion instance.
40 *
41 * @param q The quaternion to construct this quaternion instance from.
42 *****************************************************************************/
43 /*
44 CalQuaternion::CalQuaternion(const CalQuaternion& q)
45 : x(q.x), y(q.y), z(q.z), w(q.w)
46 {
47 }
48 */
49 /*****************************************************************************/
50 /** Constructs the quaternion instance.
51 *
52 * This function is a constructor of the quaternion instance.
53 *
54 * @param qx The x component.
55 * @param qy The y component.
56 * @param qz The z component.
57 * @param qw The w component.
58 *****************************************************************************/
59 /*
60 CalQuaternion::CalQuaternion(float qx, float qy, float qz, float qw)
61 : x(qx), y(qy), z(qz), w(qw)
62 {
63 }
64 */
65 /*****************************************************************************/
66 /** Destructs the quaternion instance.
67 *
68 * This function is the destructor of the quaternion instance.
69 *****************************************************************************/
70 /*
71 CalQuaternion::~CalQuaternion()
72 {
73 }
74 */
75 /*****************************************************************************/
76 /** Provides access to the components of the quaternion instance.
77 *
78 * This function provides read and write access to the three components of the
79 * quaternion instance.
80 *
81 * @param index The index to the specific component.
82 *
83 * @return A reference to the specific component.
84 *****************************************************************************/
85 /*
86 float& CalQuaternion::operator[](unsigned int index)
87 {
88 return (&x)[index];
89 }
90 */
91 /*****************************************************************************/
92 /** Provides access to the components of the quaternion instance.
93 *
94 * This function provides read access to the three components of the quaternion
95 * instance.
96 *
97 * @param index The index to the specific component.
98 *
99 * @return A constant reference to the specific component.
100 *****************************************************************************/
101 /*
102 const float& CalQuaternion::operator[](unsigned int index) const
103 {
104 return (&x)[index];
105 }
106 */
107 /*****************************************************************************/
108 /** Equates the quaternion instance with another quaternion.
109 *
110 * This operator equates the quaternion instance with another quaternion.
111 *
112 * @param q The quaternion to equate the quaternion instance with.
113 *****************************************************************************/
114 /*
115 void CalQuaternion::operator=(const CalQuaternion& q)
116 {
117 x = q.x;
118 y = q.y;
119 z = q.z;
120 w = q.w;
121 }
122 */
123 /*****************************************************************************/
124 /** Multiplies another quaternion to the quaternion instance.
125 *
126 * This operator multiplies another quaternion to the quaternion instance.
127 *
128 * @param q The quaternion to be multiplied.
129 *****************************************************************************/
130 /*
131 void CalQuaternion::operator*=(const CalQuaternion& q)
132 {
133 float qx, qy, qz, qw;
134 qx = x;
135 qy = y;
136 qz = z;
137 qw = w;
138
139 x = qw * q.x + qx * q.w + qy * q.z - qz * q.y;
140 y = qw * q.y - qx * q.z + qy * q.w + qz * q.x;
141 z = qw * q.z + qx * q.y - qy * q.x + qz * q.w;
142 w = qw * q.w - qx * q.x - qy * q.y - qz * q.z;
143 }
144 */
145 /*****************************************************************************/
146 /** Multiplies a vector to the quaternion instance.
147 *
148 * This operator multiplies a vector to the quaternion instance.
149 *
150 * @param v The vector to be multiplied.
151 *****************************************************************************/
152 /*
153 void CalQuaternion::operator*=(const CalVector& v)
154 {
155 float qx, qy, qz, qw;
156 qx = x;
157 qy = y;
158 qz = z;
159 qw = w;
160
161 x = qw * v.x + qy * v.z - qz * v.y;
162 y = qw * v.y - qx * v.z + qz * v.x;
163 z = qw * v.z + qx * v.y - qy * v.x;
164 w = - qx * v.x - qy * v.y - qz * v.z;
165 }
166 */
167 /*****************************************************************************/
168 /** Calculates the product of two quaternions.
169 *
170 * This operator calculates the product of two quaternions.
171 *
172 * @param q The first quaternion.
173 * @param r The second quaternion.
174 *
175 * @return The product of the two quaternions.
176 *****************************************************************************/
177 /*
178 CalQuaternion operator*(const CalQuaternion& q, const CalQuaternion& r)
179 {
180 return CalQuaternion(
181 r.w * q.x + r.x * q.w + r.y * q.z - r.z * q.y,
182 r.w * q.y - r.x * q.z + r.y * q.w + r.z * q.x,
183 r.w * q.z + r.x * q.y - r.y * q.x + r.z * q.w,
184 r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z
185 );
186 }
187 */
188 /*****************************************************************************/
189 /** Interpolates the quaternion instance to another quaternion.
190 *
191 * This function interpolates the quaternion instance to another quaternion by
192 * a given factor.
193 *
194 * @param d The blending factor in the range [0.0, 1.0].
195 * @param v The quaternion to be interpolated to.
196 *****************************************************************************/
197 /*
198 void CalQuaternion::blend(float d, const CalQuaternion& q)
199 {
200 float norm;
201 norm = x * q.x + y * q.y + z * q.z + w * q.w;
202
203 bool bFlip;
204 bFlip = false;
205
206 if(norm < 0.0f)
207 {
208 norm = -norm;
209 bFlip = true;
210 }
211
212 float inv_d;
213 if(1.0f - norm < 0.000001f)
214 {
215 inv_d = 1.0f - d;
216 }
217 else
218 {
219 float theta;
220 theta = (float) acos(norm);
221
222 float s;
223 s = (float) (1.0f / sin(theta));
224
225 inv_d = (float) sin((1.0f - d) * theta) * s;
226 d = (float) sin(d * theta) * s;
227 }
228
229 if(bFlip)
230 {
231 d = -d;
232 }
233
234 x = inv_d * x + d * q.x;
235 y = inv_d * y + d * q.y;
236 z = inv_d * z + d * q.z;
237 w = inv_d * w + d * q.w;
238 }
239 */
240 /*****************************************************************************/
241 /** Clears the quaternion instance.
242 *
243 * This function clears the quaternion instance.
244 *****************************************************************************/
245 /*
246 void CalQuaternion::clear()
247 {
248 x = 0.0f;
249 y = 0.0f;
250 z = 0.0f;
251 w = 1.0f;
252 }
253 */
254 /*****************************************************************************/
255 /** Conjugates the quaternion instance.
256 *
257 * This function conjugates the quaternion instance.
258 *****************************************************************************/
259 /*
260 void CalQuaternion::conjugate()
261 {
262 x = -x;
263 y = -y;
264 z = -z;
265 }
266 */
267 /*****************************************************************************/
268 /** Inverts the quaternion instance.
269 *
270 * This function inverts the quaternion instance.
271 *****************************************************************************/
272 /*
273 void CalQuaternion::invert()
274 {
275 conjugate();
276 const float norm = (x*x) + (y*y) + (z*z) + (w*w);
277
278 if (norm == 0.0f) return;
279
280 const float inv_norm = 1 / norm;
281 x *= inv_norm;
282 y *= inv_norm;
283 z *= inv_norm;
284 w *= inv_norm;
285 }
286 */
287
288 /*****************************************************************************/
289 /** Sets new values.
290 *
291 * This function sets new values in the quaternion instance.
292 *
293 * @param qx The x component.
294 * @param qy The y component.
295 * @param qz The z component.
296 * @param qw The w component.
297 *****************************************************************************/
298 /*
299 void CalQuaternion::set(float qx, float qy, float qz, float qw)
300 {
301 x = qx;
302 y = qy;
303 z = qz;
304 w = qw;
305 }
306 */
307 /*****************************************************************************/
308 /** Computes the shortest arc quaternion that will rotate one vector to another.
309 *
310 * This function finds the shortest arc quaternion.
311 * Based on equations from "Game Programming Gems" - chapter 2.10
312 *
313 * @param from The original vector
314 * @param to The target vector
315 *****************************************************************************/
316 /*
317 CalQuaternion shortestArc( const CalVector& from, const CalVector& to )
318 {
319 CalVector cross = from % to; //Compute vector cross product
320 float dot = from * to ; //Compute dot product
321
322 dot = (float) sqrt( 2*(dot+1) ) ; //We will use this equation twice
323
324 cross /= dot ; //Get the x, y, z components
325
326 //Return with the w component (Note that w is inverted because Cal3D has
327 // left-handed rotations )
328 return CalQuaternion( cross[0], cross[1], cross[2], -dot/2 ) ;
329
330 }
331 */
332 //****************************************************************************//
333
334 /*****************************************************************************/
335 /** Returns a compressed representation of the quaternion.
336 * Compresses the quaternion from traditional 4*4 = 16 bytes to 6 bytes.
337 * This is a lossy compression based on article "Quaternion, Compression"
338 * by Zarb-Adami in GPG3.
339 *
340 * @param s0 First word of the compressed quaternion.
341 * @param s1 Second word of the compressed quaternion.
342 * @param s2 Third word of the compressed quaternion.
343 *****************************************************************************/
344
compress(short & s0,short & s1,short & s2)345 void CalQuaternion::compress(short &s0, short &s1, short &s2)
346 {
347 static const float scale = 1.41421f;
348
349 if (w > x && w > y && w > z) {
350 float rx = x * scale;
351 float ry = y * scale;
352 float rz = z * scale;
353
354 if (rx < -1) rx = -1;
355 else if (rx > 1) rx = 1;
356 if (ry < -1) ry = -1;
357 else if (ry > 1) ry = 1;
358 if (rz < -1) rz = -1;
359 else if (rz > 1) rz = 1;
360
361 s0 = (short) rx * 32767;
362 s1 = (short) (int(ry * 32767) & 0xfffe) | 1;
363 s2 = (short) (int(rz * 32767) & 0xfffe) | 1;
364 }
365 else if (z > x && z > y) {
366 float rx = x * scale;
367 float ry = y * scale;
368 float rw = w * scale;
369
370 if (rx < -1) rx = -1;
371 else if (rx > 1) rx = 1;
372 if (ry < -1) ry = -1;
373 else if (ry > 1) ry = 1;
374 if (rw < -1) rw = -1;
375 else if (rw > 1) rw = 1;
376
377 s0 = (short) rx * 32767;
378 s1 = (short) (int(ry * 32767) & 0xfffe) | 1;
379 s2 = (short) (int(rw * 32767) & 0xfffe) | 0;
380 }
381 else if (y > x) {
382 float rx = x * scale;
383 float rz = z * scale;
384 float rw = w * scale;
385
386 if (rx < -1) rx = -1;
387 else if (rx > 1) rx = 1;
388 if (rz < -1) rz = -1;
389 else if (rz > 1) rz = 1;
390 if (rw < -1) rw = -1;
391 else if (rw > 1) rw = 1;
392
393 s0 = (short) rx * 32767;
394 s1 = (short) (int(rz * 32767) & 0xfffe) | 0;
395 s2 = (short) (int(rw * 32767) & 0xfffe) | 1;
396 }
397 else {
398 float ry = y * scale;
399 float rz = z * scale;
400 float rw = w * scale;
401
402 if (ry < -1) ry = -1;
403 else if (ry > 1) ry = 1;
404 if (rz < -1) rz = -1;
405 else if (rz > 1) rz = 1;
406 if (rw < -1) rw = -1;
407 else if (rw > 1) rw = 1;
408
409 s0 = (short) ry * 32767;
410 s1 = (short) (int(rz * 32767) & 0xfffe) | 0;
411 s2 = (short) (int(rw * 32767) & 0xfffe) | 0;
412 }
413 }
414
415 /*****************************************************************************/
416 /** Sets new values from a compressed quaternion.
417 * Decompresses the quaternion from 6 bytes to traditional 4*4 = 16 bytes.
418 * The compression is based on article "Quaternion, Compression"
419 * by Zarb-Adami in GPG3.
420 *
421 * @param s0 First word of the compressed quaternion.
422 * @param s1 Second word of the compressed quaternion.
423 * @param s2 Third word of the compressed quaternion.
424 *****************************************************************************/
425
decompress(short & s0,short & s1,short & s2)426 void CalQuaternion::decompress(short &s0, short &s1, short &s2)
427 {
428 int which = ((s1 & 1) << 1) | (s2 & 1);
429 s1 &= 0xfffe;
430 s2 &= 0xfffe;
431
432 static const float scale = 1.0f / 32767.0f / 1.41421f;
433
434 if (which == 3) {
435 x = s0 * scale;
436 y = s1 * scale;
437 z = s2 * scale;
438
439 w = 1 - (x*x) - (y*y) - (z*z);
440 if (w > FLT_EPSILON)
441 w = sqrt(w);
442 }
443 else if (which == 2) {
444 x = s0 * scale;
445 y = s1 * scale;
446 w = s2 * scale;
447
448 z = 1 - (x*x) - (y*y) - (w*w);
449 if (z > FLT_EPSILON)
450 z = sqrt(z);
451 }
452 else if (which == 1) {
453 x = s0 * scale;
454 z = s1 * scale;
455 w = s2 * scale;
456
457 y = 1 - (x*x) - (z*z) - (w*w);
458 if (y > FLT_EPSILON)
459 y = sqrt(y);
460 }
461 else {
462 y = s0 * scale;
463 z = s1 * scale;
464 w = s2 * scale;
465
466 x = 1 - (y*y) - (z*z) - (w*w);
467 if (x > FLT_EPSILON)
468 x = sqrt(x);
469 }
470 }
471