1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
/*******************************************************************************
* *
* Viewmol *
* *
* Q U A T E R N I O N . C *
* *
* Copyright (c) Joerg-R. Hill, October 2003 *
* *
********************************************************************************
*
* $Id: quaternion.c,v 1.2 2003/11/07 11:09:31 jrh Exp $
* $Log: quaternion.c,v $
* Revision 1.2 2003/11/07 11:09:31 jrh
* Release 2.4
*
* Revision 1.1 2000/12/10 15:14:24 jrh
* Initial revision
*
*/
#include<math.h>
#include<stdio.h>
#include<GL/gl.h>
void quaternionToMatrix(GLenum, float *);
void incrementQuaternion(double, double, double, float *);
void multQuaternions(float *, float *, float *);
void quaternionToMatrix(GLenum matrixMode, float q[4])
{
double xx, xy, xz, xw, yy, yz, yw, zz, zw, x2, y2, z2;
double mat[4][4];
x2=q[0]+q[0];
y2=q[1]+q[1];
z2=q[2]+q[2];
xx=q[0]*x2;
xy=q[0]*y2;
xz=q[0]*z2;
yy=q[1]*y2;
yz=q[1]*z2;
zz=q[2]*z2;
xw=q[3]*x2;
yw=q[3]*y2;
zw=q[3]*z2;
mat[0][0]=1.0-yy-zz;
mat[0][1]= xy-zw;
mat[0][2]= xz+yw;
mat[0][3]=0.0;
mat[1][0]= xy+zw;
mat[1][1]=1.0-xx-zz;
mat[1][2]= yz-xw;
mat[1][3]=0.0;
mat[2][0]= xz-yw;
mat[2][1]= yz+xw;
mat[2][2]=1.0-xx-yy;
mat[2][3]=0.0;
mat[3][0]=mat[3][1]=mat[3][2]=0.0;
mat[3][3]=1.0;
glMatrixMode(matrixMode);
glMultMatrixd((const double *)mat);
}
void incrementQuaternion(double x, double y, double z, float *q)
{
double torad=atan(1.0)/90.0;
double cx, cy, cz, sx, sy, sz, cycz, sysz;
float q1[4], q2[4];
x*=torad;
y*=torad;
z*=torad;
cx=cos(x);
cy=cos(y);
cz=cos(z);
sx=sin(x);
sy=sin(y);
sz=sin(z);
cycz=cy*cz;
sysz=sy*sz;
q1[0]=sx*cycz-cx*sysz;
q1[1]=cx*sy*cz+sx*cy*sz;
q1[2]=cx*cy*sz-sx*sy*cz;
q1[3]=cx*cycz+sx*sysz;
multQuaternions(q, q1, q2);
q[0]=q2[0];
q[1]=q2[1];
q[2]=q2[2];
q[3]=q2[3];
}
void multQuaternions(float *q1, float *q2, float *q3)
{
q3[0]=q1[3]*q2[0]+q1[0]*q2[3]+q1[1]*q2[2]-q1[2]*q2[1];
q3[1]=q1[3]*q2[1]+q1[1]*q2[3]+q1[2]*q2[0]-q1[0]*q2[2];
q3[2]=q1[3]*q2[2]+q1[2]*q2[3]+q1[0]*q2[1]-q1[1]*q2[0];
q3[3]=q1[3]*q2[3]-q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2];
}
|
