FreeWRL / FreeX3D 4.3.0
quaternion.c
1/*
2
3
4???
5
6*/
7
8/****************************************************************************
9 This file is part of the FreeWRL/FreeX3D Distribution.
10
11 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
12
13 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
14 it under the terms of the GNU Lesser Public License as published by
15 the Free Software Foundation, either version 3 of the License, or
16 (at your option) any later version.
17
18 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
19 but WITHOUT ANY WARRANTY; without even the implied warranty of
20 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 GNU General Public License for more details.
22
23 You should have received a copy of the GNU General Public License
24 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
25****************************************************************************/
26
27
28#include <config.h>
29#include <system.h>
30#include <display.h>
31#include <internal.h>
32
33#include <libFreeWRL.h>
34
35#include "../vrml_parser/Structs.h"
36#include "../opengl/OpenGL_Utils.h"
37#include "../main/headers.h"
38
39#include "LinearAlgebra.h"
40#include "quaternion.h"
41
42#ifndef MATH_PI
43#define MATH_PI 3.14159265358979323846
44#endif
45#ifndef DEGREES_PER_RADIAN
46#define DEGREES_PER_RADIAN (double)57.2957795130823208768
47#endif
48double rad2deg(double rad){
49 return rad * DEGREES_PER_RADIAN;
50}
51
52/*
53 * Quaternion math ported from Perl to C
54 * (originally in Quaternion.pm)
55 *
56 * VRML rotation representation:
57 *
58 * axis (x, y, z) and angle (radians), default is unit vector = (0, 0, 1)
59 * and angle = 0 (see VRML97 spec. clauses 4.4.5 'Standard units and
60 * coordinate system', 5.8 'SFRotation and MFRotation')
61 *
62 * Quaternion representation:
63 *
64 * q = w + xi + yj + zk or q = (w, v) where w is a scalar and
65 * v = (x, y, z) is a vector
66 *
67 * Quaternion addition:
68 *
69 * q1 + q2 = (w1 + w2, v1 + v2)
70 * = (w1 + w2) + (x1 + x2)i + (y1 + y2)j + (z1 + z2)k
71 *
72 * Quaternion multiplication
73 * (let the dot product of v1 and v2 be v1 dp v2):
74 *
75 * q1q2 = (w1, v1) dp (w2, v2)
76 * = (w1w2 - v1 dp v2, w1v2 + w2v1 + v1 x v2)
77 * q1q2 != q2q1 (not commutative)
78 *
79 *
80 * norm(q) = || q || = sqrt(w^2 + x^2 + y^2 + z^2) is q's magnitude
81 * normalize a quaternion: q' = q / || q ||
82 *
83 * conjugate of q = q* = (w, -v)
84 * inverse of q = q^-1 = q* / || q ||
85 * unit quaternion: || q || = 1, w^2 + x^2 + y^2 + z^2 = 1, q^-1 = q*
86 *
87 * Identity quaternions: q = (1, (0, 0, 0)) is the multiplication
88 * identity and q = (0, (0, 0, 0)) is the addition identity
89 *
90 * References:
91 *
92 * * www.gamedev.net/reference/programming/features/qpowers/
93 * * www.gamasutra.com/features/19980703/quaternions_01.htm
94 * * mathworld.wolfram.com/
95 * * skal.planet-d.net/demo/matrixfaq.htm
96 */
97
98
99
100/* change matrix rotation to/from a quaternion */
101void
102matrix_to_quaternion (Quaternion *quat, double *mat) {
103 double T, S, X, Y, Z, W;
104
105 /* get the trace of the matrix */
106 T = 1 + MAT00 + MAT11 + MAT22;
107 /* printf ("T is %f\n",T);*/
108
109 if (T > 0) {
110 S = 0.5/sqrt(T);
111 W = 0.25 / S;
112 /* x = (m21 - m12) *S*/
113 /* y = (m02 - m20) *s*/
114 /* z = (m10 - m01) *s*/
115 X=(MAT12-MAT21)*S;
116 Y=(MAT20-MAT02)*S;
117 Z=(MAT01-MAT10)*S;
118 } else {
119 /* If the trace of the matrix is equal to zero then identify*/
120 /* which major diagonal element has the greatest value.*/
121 /* Depending on this, calculate the following:*/
122
123 if ((MAT00>MAT11)&&(MAT00>MAT22)) {/* Column 0:*/
124 S = sqrt( 1.0 + MAT00 - MAT11 - MAT22 ) * 2;
125 X = 0.25 * S;
126 Y = (MAT01 + MAT10) / S;
127 Z = (MAT02 + MAT20) / S;
128 W = (MAT21 - MAT12) / S;
129 } else if ( MAT11>MAT22 ) {/* Column 1:*/
130 S = sqrt( 1.0 + MAT11 - MAT00 - MAT22) * 2;
131 X = (MAT01 + MAT10) / S;
132 Y = 0.25 * S;
133 Z = (MAT12 + MAT21) / S;
134 W = (MAT20 - MAT02) / S;
135 } else {/* Column 2:*/
136 S = sqrt( 1.0 + MAT22 - MAT00 - MAT11) * 2;
137 X = (MAT02 + MAT20) / S;
138 Y = (MAT12 + MAT21) / S;
139 Z = 0.25 * S;
140 W = (MAT10 - MAT01) / S;
141 }
142 }
143
144 /* printf ("Quat x %f y %f z %f w %f\n",X,Y,Z,W);*/
145 quat->x = X;
146 quat->y = Y;
147 quat->z = Z;
148 quat->w = W;
149}
150
151// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
152// - says vrml yz is interchanged, but not quite: theirs is RHS too,
153// http://www.euclideanspace.com/maths/standards/index.htm
154//so x->y, y->z, z->x
155// - I fixed by re-assigning their heading, attitude, bank to our yaw-pitch-roll (ypr)
157void quat2euler0(double *axyz, Quaternion *q1) {
158 double heading, attitude, bank, sqx, sqy,sqz;
159 double test = q1->x*q1->y + q1->z*q1->w;
160 if (test > 0.499) { // singularity at north pole
161 heading = 2 * atan2(q1->x,q1->w);
162 attitude = MATH_PI/2.0;
163 bank = 0;
164 axyz[0] = bank;
165 axyz[1] = heading;
166 axyz[2] = attitude;
167 return;
168 }
169 if (test < -0.499) { // singularity at south pole
170 heading = -2 * atan2(q1->x,q1->w);
171 attitude = - MATH_PI/2.0;
172 bank = 0;
173 axyz[0] = bank;
174 axyz[1] = heading;
175 axyz[2] = attitude;
176 return;
177 }
178 sqx = q1->x*q1->x;
179 sqy = q1->y*q1->y;
180 sqz = q1->z*q1->z;
181 heading = atan2(2*q1->y*q1->w - 2*q1->x*q1->z , 1.0 - 2*sqy - 2*sqz);
182 attitude = asin(2*test);
183 bank = atan2(2*q1->x*q1->w - 2*q1->y*q1->z , 1.0 - 2*sqx - 2*sqz);
184 axyz[0] = bank;
185 axyz[1] = heading;
186 axyz[2] = attitude;
187
188}
189// http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/
190void euler2quat(Quaternion *qout, double heading, double attitude, double bank) {
191 // Assuming the angles are in radians.
192 double c1,s1,c2,s2,c3,s3,c1c2,s1s2;
193
194 c1 = cos(heading *.5);
195 s1 = sin(heading *.5);
196 c2 = cos(attitude *.5);
197 s2 = sin(attitude *.5);
198 c3 = cos(bank *.5);
199 s3 = sin(bank *.5);
200 c1c2 = c1*c2;
201 s1s2 = s1*s2;
202 qout->w =c1c2*c3 - s1s2*s3; //w
203 qout->x =c1c2*s3 + s1s2*c3; //x
204 qout->y =s1*c2*c3 + c1*s2*s3; //y
205 qout->z =c1*s2*c3 - s1*c2*s3; //z
206}
207void euler2quat1(Quaternion *qout, double *axyz) {
208 double bank, heading, attitude;
209 bank = axyz[0];
210 heading = axyz[1];
211 attitude = axyz[2];
212 printf("euler2quat bank= %lf heading= %lf attitude= %lf\n",bank,heading,attitude);
213
214}
215
216int iprev(int icur, int max){
217 int ip = icur - 1;
218 if(ip < 0) ip = max;
219 return ip;
220}
221int inext(int icur, int max){
222 int ip = icur + 1;
223 if(ip > max) ip = 0;
224 return ip;
225}
226void quat2euler(double *rxyz, int iaxis_halfcircle, Quaternion *q){
227 // interesting, but if I just want yaw, pitch I still get roll, with roll
228 // eating into the yaw or pitch. I don't recommend this function.
229 // the quaternion to euler formula aren't perfectly symmetrical.
230 // One axis uses an asin()/half-circle instead of atan2(,) with a singularity at +-90 degrees
231 // and depending on which axis you want the singularity/half-circle on you would roll axes forward and back in
232 // a function like this.
233 // In this function we want singularity to show up on the iaxis_halfcircle [0,1, or 2] axis
234 // normally you would pick an axis like x=0 and stick with it
235 //returns angle about x, y, and z axes (what some would call pitch, roll, yaw)
236 int i0,i1,i2,ii;
237 Quaternion q1;
238 double axyz[3], q4[4], q14[4];
239 quat2double(q4,q);
240 ii = iaxis_halfcircle;
241 i2 = ii;
242 i1 = iprev(i2,2);
243 i0 = iprev(i1,2);
244 q14[3] = q4[3];
245 q14[0] = q4[i0];
246 q14[1] = q4[i1];
247 q14[2] = q4[i2]; //half-circle
248 double2quat(&q1,q14);
249 quat2euler0(axyz,&q1);
250 rxyz[i0] = axyz[0];
251 rxyz[i1] = axyz[1];
252 rxyz[i2] = axyz[2]; //half-circle
253}
254
255
256void quat2yawpitch(double *ypr, Quaternion *q){
257 //uses unit vectors to solve for yaw pitch
258 //this works for SSR
259 double xaxis[3], zaxis[3];
260 xaxis[1] = xaxis[2] = zaxis[0] = zaxis[1] = 0.0;
261 xaxis[0] = 1.0;
262 zaxis[2] = 1.0;
263 quaternion_rotationd(xaxis,q,xaxis);
264 quaternion_rotationd(zaxis,q,zaxis);
265 ypr[0] = atan2(xaxis[1],xaxis[0]);
266 ypr[1] = MATH_PI*.5 - atan2(zaxis[2],sqrt(zaxis[0]*zaxis[0]+zaxis[1]*zaxis[1])) ;
267 ypr[2] = 0.0;
268}
269void test_euler(){
270 double aval;
271 aval = -MATH_PI/3.0;
272
273 if(0){
274 //compound > 90 test (failed in version 1)
275 Quaternion qpitch, qyaw, qyp;
276 double rxyz[3];
277 vrmlrot_to_quaternion(&qpitch,1.0,0.0,0.0,-MATH_PI*.5);
278
279 printf("yaw 135\n");
280 vrmlrot_to_quaternion(&qyaw,0.0,0.0,1.0,MATH_PI*.75);
281 quaternion_multiply(&qyp,&qyaw,&qpitch);
282 quaternion_print(&qyp,"qyp");
283 quat2euler(rxyz,0,&qyp);
284 printf("halfaxis 0 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
285 quat2euler(rxyz,1,&qyp);
286 printf("halfaxis 1 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
287 quat2euler(rxyz,2,&qyp);
288 printf("halfaxis 2 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
289 printf("\n");
290
291 printf("yaw 45\n");
292 vrmlrot_to_quaternion(&qyaw,0.0,0.0,1.0,MATH_PI*.25);
293 quaternion_multiply(&qyp,&qyaw,&qpitch);
294 quaternion_print(&qyp,"qyp");
295 quat2euler(rxyz,0,&qyp); //<<<< works !
296 printf("halfaxis 0 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
297 quat2euler(rxyz,1,&qyp);
298 printf("halfaxis 1 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
299 quat2euler(rxyz,2,&qyp);
300 printf("halfaxis 2 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
301 printf("\n");
302
303 }
304 if(0){
305 //tests compound angle > 90 with cycle euler -> quat -> euler -> quat
306 Quaternion qpitch, qyaw, qyp;
307 double rxyz[3], dyaw, dpitch, droll;
308 int i;
309
310 dyaw = MATH_PI*.75;
311 dpitch = -MATH_PI*.5;
312 droll = 0.0;
313 printf("starting dyaw, dpitch = %lf %lf\n",rad2deg(dyaw),rad2deg(dpitch));
314 for(i=0;i<3;i++){
315 if(0){
316 vrmlrot_to_quaternion(&qpitch,1.0,0.0,0.0,dpitch);
317 vrmlrot_to_quaternion(&qyaw,0.0,0.0,1.0,dyaw);
318 quaternion_multiply(&qyp,&qyaw,&qpitch);
319 }else{
320 double axyz[3];
321 axyz[0] = dpitch,
322 axyz[1] = droll;
323 axyz[2] = dyaw;
324 euler2quat1(&qyp,axyz);
325 }
326 quaternion_print(&qyp,"qyp");
327 quat2euler(rxyz,0,&qyp);
328 printf("halfaxis 0 rxyz = [%lf %lf %lf]\n",rad2deg(rxyz[0]),rad2deg(rxyz[1]),rad2deg(rxyz[2]));
329 dpitch = rxyz[0];
330 dyaw = rxyz[2];
331 droll = 0.0;
332 printf("%d dyaw, dpitch = %lf %lf\n",i, rad2deg(dyaw),rad2deg(dpitch));
333 }
334 }
335 if(1){
336 //tests unit vector method of quat to yaw, pitch for various angles
337 Quaternion qpitch, qyaw, qyp;
338 double ypr[3], dyaw, dpitch, droll,delta;
339 int i,j;
340
341 dyaw = 0.0;
342 dpitch = 0.0;
343 droll = 0.0;
344 delta = MATH_PI *.25;
345 for(i=0;i<9;i++){
346 dyaw = (double)(i) * delta;
347 for(j=0;j<5;j++){
348 dpitch = (double)(j) * delta;
349 vrmlrot_to_quaternion(&qpitch,1.0,0.0,0.0,dpitch);
350 vrmlrot_to_quaternion(&qyaw,0.0,0.0,1.0,dyaw);
351 quaternion_multiply(&qyp,&qyaw,&qpitch);
352 quat2yawpitch(ypr,&qyp);
353 printf("yp in [%lf %lf] yp out [%lf %lf]\n",rad2deg(dyaw),rad2deg(dpitch),rad2deg(ypr[0]),rad2deg(ypr[1]));
354 }
355 }
356
357 }
358}
359
360/* http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm */
361/* note that the above web site uses "mathematicians" not "opengl" method of matrix id */
362void
363quaternion_to_matrix (double *mat, Quaternion *q) {
364 double sqw, sqx, sqy, sqz, tmp1, tmp2;
365 double invs;
366
367 /* assumes matrix is identity, or identity + transform */
368 /* assumes matrix in OpenGL format */
369 sqw = q->w*q->w;
370 sqx = q->x*q->x;
371 sqy = q->y*q->y;
372 sqz = q->z*q->z;
373
374 /* inverse square length - if the quat is not normalized; but lets do
375 this anyway */
376 invs = 1.0 / (sqx + sqy + sqz + sqw);
377
378 /* scale */
379 MATHEMATICS_MAT00 = (sqx - sqy - sqz + sqw)*invs; /* since sqw + sqx + sqy + sqz =1*/
380 MATHEMATICS_MAT11 = (-sqx + sqy - sqz + sqw)*invs;
381 MATHEMATICS_MAT22 = (-sqx - sqy + sqz + sqw)*invs;
382
383 tmp1 = q->x*q->y;
384 tmp2 = q->z*q->w;
385 MATHEMATICS_MAT10 = 2.0 * (tmp1 + tmp2) * invs; /* m[1][0]*/
386 MATHEMATICS_MAT01 = 2.0 * (tmp1 - tmp2) * invs; /* m[0][1]*/
387
388 tmp1 = q->x*q->z;
389 tmp2 = q->y*q->w;
390 MATHEMATICS_MAT20 = 2.0 * (tmp1 - tmp2) * invs; /* m[2][0]*/
391 MATHEMATICS_MAT02 = 2.0 * (tmp1 + tmp2) * invs; /* m[0][2]*/
392 tmp1 = q->y*q->z;
393 tmp2 = q->x*q->w;
394 MATHEMATICS_MAT21 = 2.0 * (tmp1 + tmp2) * invs; /* m[2][1]*/
395 MATHEMATICS_MAT12 = 2.0 * (tmp1 - tmp2) * invs; /* m[1][2]*/
396}
397
398/*
399 * VRML rotation (axis, angle) to quaternion (q = (w, v)):
400 *
401 * To simplify the math, the rotation vector needs to be normalized.
402 *
403 * q.w = cos(angle / 2);
404 * q.x = (axis.x / || axis ||) * sin(angle / 2)
405 * q.y = (axis.y / || axis ||) * sin(angle / 2)
406 * q.z = (axis.z / || axis ||) * sin(angle / 2)
407 *
408 * Normalize quaternion: q /= ||q ||
409 */
410
411void
412vrmlrot_to_quaternion(Quaternion *quat, const double x, const double y, const double z, const double a)
413{
414 double s;
415 double scale = sqrt((x * x) + (y * y) + (z * z));
416
417 /* no rotation - use (multiplication ???) identity quaternion */
418 if (APPROX(scale, 0.0)) {
419 quat->w = 1.0;
420 quat->x = 0.0;
421 quat->y = 0.0;
422 quat->z = 0.0;
423 } else {
424 s = sin(a/2.0);
425 /* normalize rotation axis to convert VRML rotation to quaternion */
426 quat->w = cos(a / 2.0);
427 quat->x = s * (x / scale);
428 quat->y = s * (y / scale);
429 quat->z = s * (z / scale);
430 quaternion_normalize(quat);
431 }
432}
433void vrmlrot4d_to_quaternion(Quaternion *quat, const double *xyza){
434 vrmlrot_to_quaternion(quat,xyza[0],xyza[1],xyza[2],xyza[3]);
435}
436void vrmlrot4f_to_quaternion(Quaternion *quat, const float *xyza){
437 vrmlrot_to_quaternion(quat,xyza[0],xyza[1],xyza[2],xyza[3]);
438}
439
440/*
441 * Quaternion (q = (w, v)) to VRML rotation (axis, angle):
442 *
443 * angle = 2 * acos(q.w)
444 * axis.x = q.x / scale
445 * axis.y = q.y / scale
446 * axis.z = q.z / scale
447 *
448 * unless scale = 0, in which case, we'll use the default VRML
449 * rotation
450 *
451 * One can use either scale = sqrt(q.x^2 + q.y^2 + q.z^2) or
452 * scale = sin(acos(q.w)).
453 * Since we are using unit quaternions, 1 = w^2 + x^2 + y^2 + z^2.
454 * Also, acos(x) = asin(sqrt(1 - x^2)) (for x >= 0, but since we don't
455 * actually compute asin(sqrt(1 - x^2)) let's not worry about it).
456 * scale = sin(acos(q.w)) = sin(asin(sqrt(1 - q.w^2)))
457 * = sqrt(1 - q.w^2) = sqrt(q.x^2 + q.y^2 + q.z^2)
458 */
459void
460quaternion_to_vrmlrot(const Quaternion *quat, double *x, double *y, double *z, double *a)
461{
462
463 //double scale = sqrt(VECSQ(*quat));
464 Quaternion qn;
465 double s2, scale;
466
467 quaternion_set(&qn,quat);
468 quaternion_normalize(&qn);
469 // Mar 2018 having some rare numerical problems in here when quat is w=1 xyz = +-0 +-0 +-0
470 // in Component_Geospatial prep_geoViewpoint
471 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
472 // does scale a bit different
473 //our scale:
474 //scale = sqrt((qn.x * qn.x) + (qn.y * qn.y) + (qn.z * qn.z));
475 //euc scale (seems to work for my problem cases):
476 //no - the problem now is sqrt(0) comes out -1.#IND
477 // or more preciesly MSVC gives NaN if value is negative, and ours might be
478 // a tiny tiny bit negative.
479 s2 = 1.0 - qn.w;
480 if (APPROX(s2, 0.0) || s2 < 0.0) {
481 *x = 0;
482 *y = 0;
483 *z = 1;
484 *a = 0;
485 } else {
486 scale = sqrt(1.0 - qn.w);
487 *x = qn.x / scale;
488 *y = qn.y / scale;
489 *z = qn.z / scale;
490 *a = 2.0 * acos(qn.w);
491 }
492}
493void quaternion_to_vrmlrot4d(const Quaternion *quat, double *xyza){
494 quaternion_to_vrmlrot(quat, &xyza[0], &xyza[1], &xyza[2], &xyza[3]);
495}
496void quaternion_to_vrmlrot4f(const Quaternion *quat, float *rot)
497{
498
499 //double scale = sqrt(VECSQ(*quat));
500 Quaternion qn;
501 double scale;
502
503 quaternion_set(&qn,quat);
504 quaternion_normalize(&qn);
505 scale = sqrt((qn.x * qn.x) + (qn.y * qn.y) + (qn.z * qn.z));
506 if (APPROX(scale, 0.0)) {
507 rot[0] = 0.0f;
508 rot[1] = 0.0f;
509 rot[2] = 1.0f;
510 rot[3] = 0.0f;
511 } else {
512 rot[0] = qn.x / scale;
513 rot[1] = qn.y / scale;
514 rot[2] = qn.z / scale;
515 rot[3] = 2.0 * acos(qn.w);
516 }
517}
518
519
520void
521quaternion_conjugate(Quaternion *quat)
522{
523 quat->x *= -1;
524 quat->y *= -1;
525 quat->z *= -1;
526}
527
528void
529quaternion_inverse(Quaternion *ret, const Quaternion *quat)
530{
531/* double n = norm(quat); */
532
533 quaternion_set(ret, quat);
534 quaternion_conjugate(ret);
535
536 /* unit quaternion, so take conjugate */
537 quaternion_normalize(ret);
538 /* printf("Quaternion inverse: ret = {%f, %f, %f, %f}, quat = {%f, %f, %f, %f}\n",
539 ret->w, ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z); */
540}
541
542double
543quaternion_norm(const Quaternion *quat)
544{
545 return(sqrt(
546 quat->w * quat->w +
547 quat->x * quat->x +
548 quat->y * quat->y +
549 quat->z * quat->z
550 ));
551}
552
553void
554quaternion_normalize(Quaternion *quat)
555{
556 double n = quaternion_norm(quat);
557 if (APPROX(n, 1.0)) {
558 return;
559 }
560 quat->w /= n;
561 quat->x /= n;
562 quat->y /= n;
563 quat->z /= n;
564}
565// adding quaternions is rarely needed but we do something like it in squad interpolator
566void quaternion_add(Quaternion *ret, const Quaternion *q1, const Quaternion *q2) {
567 double t1[3];
568 double t2[3];
569
570 /* scale_v3f (Q(*q2)[3], (v3f *) q1, &t1); */
571 t1[0] = q2->w * q1->x;
572 t1[1] = q2->w * q1->y;
573 t1[2] = q2->w * q1->z;
574
575 /* scale_v3f (Q(*q1)[3], (v3f *) q2, &t2); */
576 t2[0] = q1->w * q2->x;
577 t2[1] = q1->w * q2->y;
578 t2[2] = q1->w * q2->z;
579
580 /* add_v3f (&t1, &t2, &t1); */
581 t1[0] = t1[0] + t2[0];
582 t1[1] = t1[1] + t2[1];
583 t1[2] = t1[2] + t2[2];
584
585 /* cross_v3f ((v3f *) q2, (v3f *) q1, &t2); */
586 t2[0] = ( q2->y * q1->z - q2->z * q1->y );
587 t2[1] = ( q2->z * q1->x - q2->x * q1->z );
588 t2[2] = ( q2->x * q1->y - q2->y * q1->x );
589
590 /* add_v3f (&t1, &t2, (v3f *) dest); */
591 ret->x = t1[0] + t2[0];
592 ret->y = t1[1] + t2[1];
593 ret->z = t1[2] + t2[2];
594
595 /* Q(*dest)[3] = Q(*q1)[3] * Q(*q2)[3] - inner_v3f((v3f *) q1, (v3f *) q2); */
596 ret->w = q1->w * q2->w - ( q1->x * q2->x + q1->y * q2->y + q1->z * q2->z );
597}
598// mostly we multiply quaterions, and rotate points
599// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
600void
601quaternion_multiply(Quaternion *ret, const Quaternion *q1, const Quaternion *q2)
602{
603 Quaternion qa, qb;
604 quaternion_set(&qa,q1);
605 quaternion_set(&qb,q2);
606 ret->w = (qa.w * qb.w) - (qa.x * qb.x) - (qa.y * qb.y) - (qa.z * qb.z);
607 ret->x = (qa.w * qb.x) + (qa.x * qb.w) + (qa.y * qb.z) - (qa.z * qb.y);
608 ret->y = (qa.w * qb.y) + (qa.y * qb.w) - (qa.x * qb.z) + (qa.z * qb.x);
609 ret->z = (qa.w * qb.z) + (qa.z * qb.w) + (qa.x * qb.y) - (qa.y * qb.x);
610/* printf("Quaternion multiply: ret = {%f, %f, %f, %f}, q1 = {%f, %f, %f, %f}, q2 = {%f, %f, %f, %f}\n", ret->w, ret->x, ret->y, ret->z, q1->w, q1->x, q1->y, q1->z, q2->w, q2->x, q2->y, q2->z); */
611}
612
613void
614quaternion_scalar_multiply(Quaternion *quat, const double s)
615{
616 quat->w *= s;
617 quat->x *= s;
618 quat->y *= s;
619 quat->z *= s;
620}
621
622/*
623 * Rotate vector v by unit quaternion q:
624 *
625 * v' = q q_v q^-1, where q_v = [0, v]
626 * the so-called sandwich product p2 = q * p * q'
627 */
628void
629quaternion_rotation(struct point_XYZ *ret, const Quaternion *quat, const struct point_XYZ *v)
630{
631 Quaternion q_v, q_i, q_r1, q_r2;
632
633 q_v.w = 0.0;
634 q_v.x = v->x;
635 q_v.y = v->y;
636 q_v.z = v->z;
637 quaternion_inverse(&q_i, quat);
638 quaternion_multiply(&q_r1, &q_v, &q_i);
639 quaternion_multiply(&q_r2, quat, &q_r1);
640
641 ret->x = q_r2.x;
642 ret->y = q_r2.y;
643 ret->z = q_r2.z;
644 /* printf("Quaternion rotation: ret = {%f, %f, %f}, quat = {%f, %f, %f, %f}, v = {%f, %f, %f}\n", ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z, v->x, v->y, v->z); */
645}
646void
647quaternion_rotationd_old(double *ret, Quaternion *quat, double *v){
648 struct point_XYZ rp,vp;
649 double2pointxyz(&vp,v);
650 quaternion_rotation(&rp,quat,&vp);
651 pointxyz2double(ret,&rp);
652}
653double *
654quaternion_rotationd(double *ret, Quaternion *quat, double *v)
655{
656 Quaternion q_v, q_i, q_r1, q_r2;
657
658 q_v.w = 0.0;
659 q_v.x = v[0];
660 q_v.y = v[1];
661 q_v.z = v[2];
662 quaternion_inverse(&q_i, quat);
663 quaternion_multiply(&q_r1, &q_v, &q_i);
664 quaternion_multiply(&q_r2, quat, &q_r1);
665
666 ret[0] = q_r2.x;
667 ret[1] = q_r2.y;
668 ret[2] = q_r2.z;
669 /* printf("Quaternion rotation: ret = {%f, %f, %f}, quat = {%f, %f, %f, %f}, v = {%f, %f, %f}\n", ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z, v->x, v->y, v->z); */
670 return ret;
671}
672
673float *
674quaternion_rotation3f(float *ret, Quaternion *quat, float *v)
675{
676 Quaternion q_v, q_i, q_r1, q_r2;
677
678 q_v.w = 0.0;
679 q_v.x = v[0];
680 q_v.y = v[1];
681 q_v.z = v[2];
682 quaternion_inverse(&q_i, quat);
683 quaternion_multiply(&q_r1, &q_v, &q_i);
684 quaternion_multiply(&q_r2, quat, &q_r1);
685
686 ret[0] = q_r2.x;
687 ret[1] = q_r2.y;
688 ret[2] = q_r2.z;
689 /* printf("Quaternion rotation: ret = {%f, %f, %f}, quat = {%f, %f, %f, %f}, v = {%f, %f, %f}\n", ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z, v->x, v->y, v->z); */
690 return ret;
691}
692
693void
694quaternion_togl(Quaternion *quat)
695{
696 if (APPROX(fabs(quat->w), 1)) { return; }
697
698 if (quat->w > 1) { quaternion_normalize(quat); }
699
700 /* get the angle, but turn us around 180 degrees */
701 /* printf ("togl: setting rotation %f %f %f %f\n",quat->w,quat->x,quat->y,quat->z);*/
702 FW_GL_ROTATE_RADIANS(2.0 * acos(quat->w), quat->x, quat->y, quat->z);
703}
704
705void
706quaternion_set(Quaternion *ret, const Quaternion *quat)
707{
708 ret->w = quat->w;
709 ret->x = quat->x;
710 ret->y = quat->y;
711 ret->z = quat->z;
712}
713
714/*
715 * Code from www.gamasutra.com/features/19980703/quaternions_01.htm,
716 * Listing 5.
717 *
718 * SLERP(p, q, t) = [p sin((1 - t)a) + q sin(ta)] / sin(a)
719 *
720 * where a is the arc angle, quaternions pq = cos(q) and 0 <= t <= 1
721 */
722void
723quaternion_slerp(Quaternion *ret, const Quaternion *q1, const Quaternion *q2, const double t)
724{
725 double omega, cosom, sinom, scale0, scale1, q2_array[4];
726
727 cosom =
728 q1->x * q2->x +
729 q1->y * q2->y +
730 q1->z * q2->z +
731 q1->w * q2->w;
732
733 if (cosom < 0.0) {
734 cosom = -cosom;
735 q2_array[0] = -q2->x;
736 q2_array[1] = -q2->y;
737 q2_array[2] = -q2->z;
738 q2_array[3] = -q2->w;
739 } else {
740 q2_array[0] = q2->x;
741 q2_array[1] = q2->y;
742 q2_array[2] = q2->z;
743 q2_array[3] = q2->w;
744 }
745
746 /* calculate coefficients */
747 if ((1.0 - cosom) > DELTA) {
748 /* standard case (SLERP) */
749 omega = acos(cosom);
750 sinom = sin(omega);
751 scale0 = sin((1.0 - t) * omega) / sinom;
752 scale1 = sin(t * omega) / sinom;
753 } else {
754 /* q1 & q2 are very close, so do linear interpolation */
755 scale0 = 1.0 - t;
756 scale1 = t;
757 }
758 ret->x = scale0 * q1->x + scale1 * q2_array[0];
759 ret->y = scale0 * q1->y + scale1 * q2_array[1];
760 ret->z = scale0 * q1->z + scale1 * q2_array[2];
761 ret->w = scale0 * q1->w + scale1 * q2_array[3];
762}
763void quaternion_print(const Quaternion *quat, char* description ){
764 printf("quat %s",description);
765 printf(" xyzw=[%lf %lf %lf %lf]\n",quat->x,quat->y,quat->z,quat->w);
766}
767void double2quat(Quaternion *quat, double *quat4){
768 quat->x = quat4[0];
769 quat->y = quat4[1];
770 quat->z = quat4[2];
771 quat->w = quat4[3];
772}
773void quat2double(double *quat4,Quaternion *quat){
774 quat4[0] = quat->x;
775 quat4[1] = quat->y;
776 quat4[2] = quat->z;
777 quat4[3] = quat->w;
778}
779
780void vrmlrot_normalize(float *ret)
781{
782 float s = ret[0]*ret[0] + ret[1]*ret[1] + ret[2]*ret[2];
783 s = (float) sqrt(s);
784 if( s != 0.0f )
785 {
786 s = 1.0f/s;
787 ret[0] *= s;
788 ret[1] *= s;
789 ret[2] *= s;
790 }
791 else
792 {
793 ret[2] = 1.0f;
794 }
795 ret[3] = (float) atan2(sin(ret[3]), cos(ret[3]));
796}
797
798void vrmlrot_multiply(float* ret, float *a, float *b)
799{
800 ret[0] = (b[0] * b[3]) + (a[0] * a[3]);
801 ret[1] = (b[1] * b[3]) + (a[1] * a[3]);
802 ret[2] = (b[2] * b[3]) + (a[2] * a[3]);
803 ret[3] = (float) sqrt(ret[0]*ret[0] + ret[1]*ret[1] + ret[2]*ret[2]);
804 if (ret[3] == 0.0f)
805 {
806 ret[0] = 1.0f;
807 ret[1]=ret[2] = 0.0f;
808 return;
809 }
810 ret[0] = ret[0]/ret[3];
811 ret[1] = ret[1]/ret[3];
812 ret[2] = ret[2]/ret[3];
813 //vrmlrot_normalize(ret);
814}
815