1 #include "math.h"
2 #include "stdio.h"
3 #include "vector.h"
4
5 #include "myglutaux.h"
6
7 #include "quaternion.h"
8
9
Quaternion()10 Quaternion::Quaternion()
11 {
12 w=1;
13 x=y=z=0;
14 } /* Quaternion::Quaternion */
15
16
Quaternion(FILE * fp)17 Quaternion::Quaternion(FILE *fp)
18 {
19 load(fp);
20 normalize();
21 } /* Quaternion::Quaternion */
22
23
load(FILE * fp)24 bool Quaternion::load(FILE *fp)
25 {
26 float t1,t2,t3,t4;
27
28 if (4!=fscanf(fp,"%f %f %f %f",&t1,&t2,&t3,&t4)) return false;
29 w=t1;
30 x=t2;
31 y=t3;
32 z=t4;
33
34
35 return true;
36 } /* Quaternion::load */
37
38
save(FILE * fp)39 bool Quaternion::save(FILE *fp)
40 {
41 fprintf(fp,"%f %f %f %f\n",float(w),float(x),float(y),float(z));
42
43 return true;
44 } /* Quaternion::save */
45
46
to_matrix(float * m)47 void Quaternion::to_matrix(float *m)
48 {
49 m[0]=float(1-2*y*y-2*z*z);
50 m[1]=float(2*x*y+2*w*z);
51 m[2]=float(2*x*z-2*w*y);
52 m[3]=0;
53
54 m[4]=float(2*x*y-2*w*z);
55 m[5]=float(1-2*x*x-2*z*z);
56 m[6]=float(2*y*z+2*w*x);
57 m[7]=0;
58
59 m[8]=float(2*x*z+2*w*y);
60 m[9]=float(2*y*z-2*w*x);
61 m[10]=float(1-2*x*x-2*y*y);
62 m[11]=0;
63
64 m[12]=0;
65 m[13]=0;
66 m[14]=0;
67 m[15]=1;
68 } /* Quaternion::to_matrix */
69
70
to_axis_angle(float * axis,float * angle)71 void Quaternion::to_axis_angle(float *axis,float *angle)
72 {
73 double scale;
74
75 normalize();
76
77 *angle=float(2*acos(w));
78 scale=x*x+y*y+z*z;
79 if (scale==0) {
80 axis[0]=0;
81 axis[1]=0;
82 axis[2]=1;
83 } else {
84 axis[0]=float(x/scale);
85 axis[1]=float(y/scale);
86 axis[2]=float(z/scale);
87 } /* if */
88 } /* Quaternion::to_axis_angle */
89
90
from_axis_angle(float * axis,float angle)91 void Quaternion::from_axis_angle(float *axis,float angle)
92 {
93 double sa;
94
95 sa=sin(angle/2);
96
97 w=cos(angle/2);
98 x=axis[0]*sa;
99 y=axis[1]*sa;
100 z=axis[2]*sa;
101 normalize();
102 } /* from_axis_angle */
103
104
to_axis_angle(Vector * axis,double * angle)105 void Quaternion::to_axis_angle(Vector *axis,double *angle)
106 {
107 double scale;
108
109 normalize();
110
111 *angle=float(2*acos(w));
112 scale=x*x+y*y+z*z;
113 if (scale==0) {
114 axis->x=0;
115 axis->y=0;
116 axis->z=1;
117 } else {
118 axis->x=x/scale;
119 axis->y=y/scale;
120 axis->z=z/scale;
121 } /* if */
122 } /* Quaternion::to_axis_angle */
123
124
from_axis_angle(Vector axis,double angle)125 void Quaternion::from_axis_angle(Vector axis,double angle)
126 {
127 double sa;
128
129 sa=sin(angle/2);
130
131 w=cos(angle/2);
132 x=axis.x*sa;
133 y=axis.y*sa;
134 z=axis.z*sa;
135 normalize();
136 } /* from_axis_angle */
137
138
post_multiply(Quaternion * q)139 void Quaternion::post_multiply(Quaternion *q)
140 {
141 double ww,xx,yy,zz;
142
143 ww=w*q->w - x*q->x - y*q->y - z*q->z;
144 xx=w*q->x + x*q->w + y*q->z - z*q->y;
145 yy=w*q->y + y*q->w + z*q->x - x*q->z;
146 zz=w*q->z + z*q->w + x*q->y - y*q->x;
147
148 w=ww;
149 x=xx;
150 y=yy;
151 z=zz;
152 normalize();
153 } /* Quaternion::multiply */
154
155
pre_multiply(Quaternion * q)156 void Quaternion::pre_multiply(Quaternion *q)
157 {
158 double ww,xx,yy,zz;
159
160 ww=q->w*w - q->x*x - q->y*y - q->z*z;
161 xx=q->w*x + q->x*w + q->y*z - q->z*y;
162 yy=q->w*y + q->y*w + q->z*x - q->x*z;
163 zz=q->w*z + q->z*w + q->x*y - q->y*x;
164
165 w=ww;
166 x=xx;
167 y=yy;
168 z=zz;
169 normalize();
170 } /* Quaternion::multiply */
171
172
rotate_vector(float * in,float * out)173 void Quaternion::rotate_vector(float *in,float *out)
174 {
175 float matrix[16];
176
177 normalize();
178 to_matrix(matrix);
179 ApplyMatrix(in,matrix,out);
180 } /* Quaternion::rotate_vector */
181
182
183
rotate_vector(Vector in,Vector & out)184 void Quaternion::rotate_vector(Vector in,Vector &out)
185 {
186 float inv[4]={float(in.x),float(in.y),float(in.z),0},outv[4]={float(out.x),float(out.y),float(out.z),0};
187 float matrix[16];
188
189 normalize();
190 to_matrix(matrix);
191 ApplyMatrix(inv,matrix,outv);
192 out.x=outv[0];
193 out.y=outv[1];
194 out.z=outv[2];
195 } /* Quaternion::rotate_vector */
196
197
normalize(void)198 void Quaternion::normalize(void)
199 {
200 double norm;
201
202 norm=sqrt(w*w+x*x+y*y+z*z);
203 if (norm!=0) {
204 w/=norm;
205 x/=norm;
206 y/=norm;
207 z/=norm;
208 } else {
209 w=1;
210 x=y=z=0;
211 } /* if */
212 } /* Quaternion::normalize */
213
print(void)214 void Quaternion::print(void)
215 {
216 printf("[%g,(%g,%g,%g)]",w,x,y,z);
217 } /* Quaternion::print */
218
219
operator ==(Quaternion & q)220 bool Quaternion::operator==(Quaternion &q)
221 {
222 if (w!=q.w ||
223 x!=q.x ||
224 y!=q.y ||
225 z!=q.z) return false;
226
227 return true;
228 } /* Quaternion::operator== */
229
230
operator !=(Quaternion & q)231 bool Quaternion::operator!=(Quaternion &q)
232 {
233 if (w!=q.w ||
234 x!=q.x ||
235 y!=q.y ||
236 z!=q.z) return true;
237
238 return false;
239 } /* Quaternion::operator!= */
240