33#include <libFreeWRL.h>
35#include "../vrml_parser/Structs.h"
36#include "../opengl/OpenGL_Utils.h"
37#include "../main/headers.h"
39#include "LinearAlgebra.h"
40#include "quaternion.h"
43#define MATH_PI 3.14159265358979323846
45#ifndef DEGREES_PER_RADIAN
46#define DEGREES_PER_RADIAN (double)57.2957795130823208768
48double rad2deg(
double rad){
49 return rad * DEGREES_PER_RADIAN;
102matrix_to_quaternion (
Quaternion *quat,
double *mat) {
103 double T, S, X, Y, Z, W;
106 T = 1 + MAT00 + MAT11 + MAT22;
123 if ((MAT00>MAT11)&&(MAT00>MAT22)) {
124 S = sqrt( 1.0 + MAT00 - MAT11 - MAT22 ) * 2;
126 Y = (MAT01 + MAT10) / S;
127 Z = (MAT02 + MAT20) / S;
128 W = (MAT21 - MAT12) / S;
129 }
else if ( MAT11>MAT22 ) {
130 S = sqrt( 1.0 + MAT11 - MAT00 - MAT22) * 2;
131 X = (MAT01 + MAT10) / S;
133 Z = (MAT12 + MAT21) / S;
134 W = (MAT20 - MAT02) / S;
136 S = sqrt( 1.0 + MAT22 - MAT00 - MAT11) * 2;
137 X = (MAT02 + MAT20) / S;
138 Y = (MAT12 + MAT21) / S;
140 W = (MAT10 - MAT01) / S;
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;
161 heading = 2 * atan2(q1->x,q1->w);
162 attitude = MATH_PI/2.0;
170 heading = -2 * atan2(q1->x,q1->w);
171 attitude = - MATH_PI/2.0;
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);
190void euler2quat(
Quaternion *qout,
double heading,
double attitude,
double bank) {
192 double c1,s1,c2,s2,c3,s3,c1c2,s1s2;
194 c1 = cos(heading *.5);
195 s1 = sin(heading *.5);
196 c2 = cos(attitude *.5);
197 s2 = sin(attitude *.5);
202 qout->w =c1c2*c3 - s1s2*s3;
203 qout->x =c1c2*s3 + s1s2*c3;
204 qout->y =s1*c2*c3 + c1*s2*s3;
205 qout->z =c1*s2*c3 - s1*c2*s3;
207void euler2quat1(
Quaternion *qout,
double *axyz) {
208 double bank, heading, attitude;
212 printf(
"euler2quat bank= %lf heading= %lf attitude= %lf\n",bank,heading,attitude);
216int iprev(
int icur,
int max){
221int inext(
int icur,
int max){
226void quat2euler(
double *rxyz,
int iaxis_halfcircle,
Quaternion *q){
238 double axyz[3], q4[4], q14[4];
240 ii = iaxis_halfcircle;
248 double2quat(&q1,q14);
249 quat2euler0(axyz,&q1);
256void quat2yawpitch(
double *ypr,
Quaternion *q){
259 double xaxis[3], zaxis[3];
260 xaxis[1] = xaxis[2] = zaxis[0] = zaxis[1] = 0.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])) ;
277 vrmlrot_to_quaternion(&qpitch,1.0,0.0,0.0,-MATH_PI*.5);
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]));
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);
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]));
307 double rxyz[3], dyaw, dpitch, droll;
311 dpitch = -MATH_PI*.5;
313 printf(
"starting dyaw, dpitch = %lf %lf\n",rad2deg(dyaw),rad2deg(dpitch));
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);
324 euler2quat1(&qyp,axyz);
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]));
332 printf(
"%d dyaw, dpitch = %lf %lf\n",i, rad2deg(dyaw),rad2deg(dpitch));
338 double ypr[3], dyaw, dpitch, droll,delta;
344 delta = MATH_PI *.25;
346 dyaw = (double)(i) * delta;
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]));
363quaternion_to_matrix (
double *mat,
Quaternion *q) {
364 double sqw, sqx, sqy, sqz, tmp1, tmp2;
376 invs = 1.0 / (sqx + sqy + sqz + sqw);
379 MATHEMATICS_MAT00 = (sqx - sqy - sqz + sqw)*invs;
380 MATHEMATICS_MAT11 = (-sqx + sqy - sqz + sqw)*invs;
381 MATHEMATICS_MAT22 = (-sqx - sqy + sqz + sqw)*invs;
385 MATHEMATICS_MAT10 = 2.0 * (tmp1 + tmp2) * invs;
386 MATHEMATICS_MAT01 = 2.0 * (tmp1 - tmp2) * invs;
390 MATHEMATICS_MAT20 = 2.0 * (tmp1 - tmp2) * invs;
391 MATHEMATICS_MAT02 = 2.0 * (tmp1 + tmp2) * invs;
394 MATHEMATICS_MAT21 = 2.0 * (tmp1 + tmp2) * invs;
395 MATHEMATICS_MAT12 = 2.0 * (tmp1 - tmp2) * invs;
412vrmlrot_to_quaternion(
Quaternion *quat,
const double x,
const double y,
const double z,
const double a)
415 double scale = sqrt((x * x) + (y * y) + (z * z));
418 if (APPROX(scale, 0.0)) {
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);
433void vrmlrot4d_to_quaternion(
Quaternion *quat,
const double *xyza){
434 vrmlrot_to_quaternion(quat,xyza[0],xyza[1],xyza[2],xyza[3]);
436void vrmlrot4f_to_quaternion(
Quaternion *quat,
const float *xyza){
437 vrmlrot_to_quaternion(quat,xyza[0],xyza[1],xyza[2],xyza[3]);
460quaternion_to_vrmlrot(
const Quaternion *quat,
double *x,
double *y,
double *z,
double *a)
467 quaternion_set(&qn,quat);
468 quaternion_normalize(&qn);
480 if (APPROX(s2, 0.0) || s2 < 0.0) {
486 scale = sqrt(1.0 - qn.w);
490 *a = 2.0 * acos(qn.w);
493void quaternion_to_vrmlrot4d(
const Quaternion *quat,
double *xyza){
494 quaternion_to_vrmlrot(quat, &xyza[0], &xyza[1], &xyza[2], &xyza[3]);
496void quaternion_to_vrmlrot4f(
const Quaternion *quat,
float *rot)
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)) {
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);
533 quaternion_set(ret, quat);
534 quaternion_conjugate(ret);
537 quaternion_normalize(ret);
556 double n = quaternion_norm(quat);
557 if (APPROX(n, 1.0)) {
571 t1[0] = q2->w * q1->x;
572 t1[1] = q2->w * q1->y;
573 t1[2] = q2->w * q1->z;
576 t2[0] = q1->w * q2->x;
577 t2[1] = q1->w * q2->y;
578 t2[2] = q1->w * q2->z;
581 t1[0] = t1[0] + t2[0];
582 t1[1] = t1[1] + t2[1];
583 t1[2] = t1[2] + t2[2];
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 );
591 ret->x = t1[0] + t2[0];
592 ret->y = t1[1] + t2[1];
593 ret->z = t1[2] + t2[2];
596 ret->w = q1->w * q2->w - ( q1->x * q2->x + q1->y * q2->y + q1->z * q2->z );
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);
614quaternion_scalar_multiply(
Quaternion *quat,
const double s)
637 quaternion_inverse(&q_i, quat);
638 quaternion_multiply(&q_r1, &q_v, &q_i);
639 quaternion_multiply(&q_r2, quat, &q_r1);
647quaternion_rotationd_old(
double *ret,
Quaternion *quat,
double *v){
649 double2pointxyz(&vp,v);
650 quaternion_rotation(&rp,quat,&vp);
651 pointxyz2double(ret,&rp);
654quaternion_rotationd(
double *ret,
Quaternion *quat,
double *v)
662 quaternion_inverse(&q_i, quat);
663 quaternion_multiply(&q_r1, &q_v, &q_i);
664 quaternion_multiply(&q_r2, quat, &q_r1);
674quaternion_rotation3f(
float *ret,
Quaternion *quat,
float *v)
682 quaternion_inverse(&q_i, quat);
683 quaternion_multiply(&q_r1, &q_v, &q_i);
684 quaternion_multiply(&q_r2, quat, &q_r1);
696 if (APPROX(fabs(quat->w), 1)) {
return; }
698 if (quat->w > 1) { quaternion_normalize(quat); }
702 FW_GL_ROTATE_RADIANS(2.0 * acos(quat->w), quat->x, quat->y, quat->z);
725 double omega, cosom, sinom, scale0, scale1, q2_array[4];
735 q2_array[0] = -q2->x;
736 q2_array[1] = -q2->y;
737 q2_array[2] = -q2->z;
738 q2_array[3] = -q2->w;
747 if ((1.0 - cosom) > DELTA) {
751 scale0 = sin((1.0 - t) * omega) / sinom;
752 scale1 = sin(t * omega) / sinom;
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];
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);
767void double2quat(
Quaternion *quat,
double *quat4){
773void quat2double(
double *quat4,
Quaternion *quat){
780void vrmlrot_normalize(
float *ret)
782 float s = ret[0]*ret[0] + ret[1]*ret[1] + ret[2]*ret[2];
795 ret[3] = (float) atan2(sin(ret[3]), cos(ret[3]));
798void vrmlrot_multiply(
float* ret,
float *a,
float *b)
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]);
807 ret[1]=ret[2] = 0.0f;
810 ret[0] = ret[0]/ret[3];
811 ret[1] = ret[1]/ret[3];
812 ret[2] = ret[2]/ret[3];