1 /*
2 * fx/Math3d.cpp
3 *
4 * Copyright 2009 Peter Barth
5 *
6 * This file is part of Milkytracker.
7 *
8 * Milkytracker is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation, either version 3 of the License, or
11 * (at your option) any later version.
12 *
13 * Milkytracker is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with Milkytracker. If not, see <http://www.gnu.org/licenses/>.
20 *
21 */
22
23 #include "Math3d.h"
24
25 /////////////////////
26 // VectorFP struct //
27 /////////////////////
28
operator *(pp_int32 s,VectorFP v2)29 VectorFP operator*(pp_int32 s,VectorFP v2)
30 {
31 VectorFP v = {fpmul(v2.x,s),fpmul(v2.y,s),fpmul(v2.z,s)};
32 return v;
33 }
34
operator +(VectorFP v1,VectorFP v2)35 VectorFP operator+ (VectorFP v1,VectorFP v2)
36 {
37 VectorFP v = {v1.x+v2.x,v1.y+v2.y,v1.z+v2.z};
38 return v;
39 }
40
operator -(VectorFP v1,VectorFP v2)41 VectorFP operator- (VectorFP v1,VectorFP v2)
42 {
43 VectorFP v = {v1.x-v2.x,v1.y-v2.y,v1.z-v2.z};
44 return v;
45 }
46
operator -(VectorFP v)47 VectorFP operator- (VectorFP v)
48 {
49 VectorFP vr = {-v.x,-v.y,-v.z};
50 return vr;
51 }
52
operator *(VectorFP v1,VectorFP v2)53 pp_int32 operator* (VectorFP v1,VectorFP v2)
54 {
55 return fpmul(v1.x,v2.x)+fpmul(v1.y,v2.y)+fpmul(v1.z,v2.z);
56 }
57
58 // vector product
operator ^(VectorFP v1,VectorFP v2)59 VectorFP operator ^(VectorFP v1,VectorFP v2)
60 {
61 VectorFP v;
62
63 v.x=fpmul(v1.y,v2.z)-fpmul(v1.z,v2.y);
64 v.y=fpmul(v1.z,v2.x)-fpmul(v1.x,v2.z);
65 v.z=fpmul(v1.x,v2.y)-fpmul(v1.y,v2.x);
66
67 return v;
68 }
69
70 ///////////////////////
71 // VectorFloat class //
72 ///////////////////////
73
VectorFloat()74 VectorFloat::VectorFloat()
75 {
76 }
77
VectorFloat(float nx,float ny,float nz)78 VectorFloat::VectorFloat(float nx,float ny,float nz)
79 {
80 x = nx; y = ny; z = nz;
81 }
82
VectorFloat(VectorFP vector)83 VectorFloat::VectorFloat(VectorFP vector)
84 {
85 x = vector.x*(1.0f/65536.0f);
86 y = vector.y*(1.0f/65536.0f);
87 z = vector.z*(1.0f/65536.0f);
88 }
89
set(float nx,float ny,float nz)90 void VectorFloat::set(float nx,float ny,float nz)
91 {
92 x = nx; y = ny; z = nz;
93 }
94
set(VectorFP vector)95 void VectorFloat::set(VectorFP vector)
96 {
97 x = vector.x*(1.0f/65536.0f);
98 y = vector.y*(1.0f/65536.0f);
99 z = vector.z*(1.0f/65536.0f);
100 }
101
length()102 float VectorFloat::length()
103 {
104 return (float)sqrt(x*x+y*y+z*z);
105 }
106
normalize()107 void VectorFloat::normalize()
108 {
109 float len = (float)sqrt(x*x+y*y+z*z);
110 if (len>0.0f)
111 {
112 x/=len;
113 y/=len;
114 z/=len;
115 }
116 }
117
readFromFile(FILE * f)118 void VectorFloat::readFromFile(FILE *f)
119 {
120 fread(&x,4,1,f);
121 fread(&y,4,1,f);
122 fread(&z,4,1,f);
123 }
124
convertToFixedPoint()125 VectorFP VectorFloat::convertToFixedPoint()
126 {
127 VectorFP v;
128 v.x = (pp_int32 )(x*65536.0f);
129 v.y = (pp_int32 )(y*65536.0f);
130 v.z = (pp_int32 )(z*65536.0f);
131 return v;
132 }
133
operator *(float s,VectorFloat v2)134 VectorFloat operator*(float s,VectorFloat v2)
135 {
136 VectorFloat v(v2.x*s,v2.y*s,v2.z*s);
137 return v;
138 }
139
operator +(VectorFloat v1,VectorFloat v2)140 VectorFloat operator+ (VectorFloat v1,VectorFloat v2)
141 {
142 VectorFloat v(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);
143 return v;
144 }
145
operator -(VectorFloat v1,VectorFloat v2)146 VectorFloat operator- (VectorFloat v1,VectorFloat v2)
147 {
148 VectorFloat v(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);
149 return v;
150 }
151
operator -(VectorFloat v)152 VectorFloat operator- (VectorFloat v)
153 {
154 VectorFloat vr(-v.x,-v.y,-v.z);
155 return vr;
156 }
157
operator *(VectorFloat v1,VectorFloat v2)158 float operator* (VectorFloat v1,VectorFloat v2)
159 {
160 return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;
161 }
162
163 // vector product
operator ^(VectorFloat v1,VectorFloat v2)164 VectorFloat operator ^(VectorFloat v1,VectorFloat v2)
165 {
166 VectorFloat v;
167
168 v.x=(v1.y*v2.z)-(v1.z*v2.y);
169 v.y=(v1.z*v2.x)-(v1.x*v2.z);
170 v.z=(v1.x*v2.y)-(v1.y*v2.x);
171
172 return v;
173 }
174
175
176 ///////////////////////
177 // MatrixFloat class //
178 ///////////////////////
179
setID()180 void MatrixFloat::setID()
181 {
182 form[0][0] = 1.0f; form[0][1] = 0.0f; form[0][2] = 0.0f; form[0][3] = 0.0f;
183 form[1][0] = 0.0f; form[1][1] = 1.0f; form[1][2] = 0.0f; form[1][3] = 0.0f;
184 form[2][0] = 0.0f; form[2][1] = 0.0f; form[2][2] = 1.0f; form[2][3] = 0.0f;
185 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
186 }
187
setRotX(float phi)188 void MatrixFloat::setRotX(float phi)
189 {
190 form[0][0] = 1.0f; form[0][1] = 0.0f; form[0][2] = 0.0f; form[0][3] = 0.0f;
191 form[1][0] = 0.0f; form[1][1] = (float)cos(phi); form[1][2] = -(float)sin(phi); form[1][3] = 0.0f;
192 form[2][0] = 0.0f; form[2][1] = (float)sin(phi); form[2][2] = (float)cos(phi); form[2][3] = 0.0f;
193 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
194 }
195
setRotY(float phi)196 void MatrixFloat::setRotY(float phi)
197 {
198 form[0][0] = (float)cos(phi); form[0][1] = 0.0f; form[0][2] = (float)sin(phi); form[0][3] = 0.0f;
199 form[1][0] = 0.0f; form[1][1] = 1.0f; form[1][2] = 0.0f; form[1][3] = 0.0f;
200 form[2][0] = -(float)sin(phi); form[2][1] = 0.0f; form[2][2] = (float)cos(phi); form[2][3] = 0.0f;
201 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
202 }
203
setRotZ(float phi)204 void MatrixFloat::setRotZ(float phi)
205 {
206 form[0][0] = (float)cos(phi); form[0][1] = -(float)sin(phi); form[0][2] = 0.0f; form[0][3] = 0.0f;
207 form[1][0] = (float)sin(phi); form[1][1] = (float)cos(phi); form[1][2] = 0.0f; form[1][3] = 0.0f;
208 form[2][0] = 0.0f; form[2][1] = 0.0f; form[2][2] = 1.0f; form[2][3] = 0.0f;
209 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
210 }
211
setScale(VectorFloat v)212 void MatrixFloat::setScale(VectorFloat v)
213 {
214 form[0][0] = v.x; form[0][1] = 0.0f; form[0][2] = 0.0f; form[0][3] = 0.0f;
215 form[1][0] = 0.0f; form[1][1] = v.y; form[1][2] = 0.0f; form[1][3] = 0.0f;
216 form[2][0] = 0.0f; form[2][1] = 0.0f; form[2][2] = v.z; form[2][3] = 0.0f;
217 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
218 }
219
setTranslate(VectorFloat v)220 void MatrixFloat::setTranslate(VectorFloat v)
221 {
222 form[0][0] = 1.0f; form[0][1] = 0.0f; form[0][2] = 0.0f; form[0][3] = v.x;
223 form[1][0] = 0.0f; form[1][1] = 1.0f; form[1][2] = 0.0f; form[1][3] = v.y;
224 form[2][0] = 0.0f; form[2][1] = 0.0f; form[2][2] = 1.0f; form[2][3] = v.z;
225 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
226 }
227
setSubtract(VectorFloat v)228 void MatrixFloat::setSubtract(VectorFloat v)
229 {
230 form[0][0] = 1.0f; form[0][1] = 0.0f; form[0][2] = 0.0f; form[0][3] = -v.x;
231 form[1][0] = 0.0f; form[1][1] = 1.0f; form[1][2] = 0.0f; form[1][3] = -v.y;
232 form[2][0] = 0.0f; form[2][1] = 0.0f; form[2][2] = 1.0f; form[2][3] = -v.z;
233 form[3][0] = 0.0f; form[3][1] = 0.0f; form[3][2] = 0.0f; form[3][3] = 1.0f;
234 }
235
operator *(MatrixFloat m1,MatrixFloat m2)236 MatrixFloat operator* (MatrixFloat m1,MatrixFloat m2)
237 {
238 pp_int32 i,j,k;
239
240 MatrixFloat mr;
241
242 for (i = 0; i < 4; i++)
243 for (j = 0; j < 4; j++)
244 {
245 mr.form[i][j] = 0.0f;
246 for (k = 0; k < 4; k++)
247 mr.form[i][j] += m1.form[i][k]*m2.form[k][j];
248 }
249
250 return mr;
251
252 }
253
operator *(MatrixFloat m,VectorFloat v)254 VectorFloat operator* (MatrixFloat m,VectorFloat v)
255 {
256 VectorFloat vr;
257
258 vr.x = (m.form[0][0]*v.x)+(m.form[0][1]*v.y)+(m.form[0][2]*v.z)+(m.form[0][3]*1.0f);
259 vr.y = (m.form[1][0]*v.x)+(m.form[1][1]*v.y)+(m.form[1][2]*v.z)+(m.form[1][3]*1.0f);
260 vr.z = (m.form[2][0]*v.x)+(m.form[2][1]*v.y)+(m.form[2][2]*v.z)+(m.form[2][3]*1.0f);
261
262 return vr;
263 }
264
265 ////////////////////
266 // MatrixFP class //
267 ////////////////////
268
269 // default constructor
MatrixFP()270 MatrixFP::MatrixFP()
271 {
272 }
273
274 // convert floating point matrix into fixed point matrix
MatrixFP(MatrixFloat matrix)275 MatrixFP::MatrixFP(MatrixFloat matrix)
276 {
277 for (pp_int32 i = 0; i < 4; i++)
278 for (pp_int32 j = 0; j < 4; j++)
279 form[i][j] = (pp_int32 )(matrix.form[i][j]*65536.0f);
280 }
281
setID()282 void MatrixFP::setID()
283 {
284 form[0][0] = 65536; form[0][1] = 0; form[0][2] = 0; form[0][3] = 0;
285 form[1][0] = 0; form[1][1] = 65536; form[1][2] = 0; form[1][3] = 0;
286 form[2][0] = 0; form[2][1] = 0; form[2][2] = 65536; form[2][3] = 0;
287 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
288 }
289
setRotX(float phi)290 void MatrixFP::setRotX(float phi)
291 {
292 form[0][0] = 65536; form[0][1] = 0; form[0][2] = 0; form[0][3] = 0;
293 form[1][0] = 0; form[1][1] = (pp_int32 )(cos(phi)*65536.0f); form[1][2] = (pp_int32 )(-sin(phi)*65536.0f); form[1][3] = 0;
294 form[2][0] = 0; form[2][1] = (pp_int32 )(sin(phi)*65536.0f); form[2][2] = (pp_int32 )(cos(phi)*65536); form[2][3] = 0;
295 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
296 }
297
setRotY(float phi)298 void MatrixFP::setRotY(float phi)
299 {
300 form[0][0] = (pp_int32 )(cos(phi)*65536.0f); form[0][1] = 0; form[0][2] = (pp_int32 )(sin(phi)*65536.0f); form[0][3] = 0;
301 form[1][0] = 0; form[1][1] = 65536; form[1][2] = 0; form[1][3] = 0;
302 form[2][0] = (pp_int32 )(-sin(phi)*65536.0f); form[2][1] = 0; form[2][2] = (pp_int32 )(cos(phi)*65536.0f); form[2][3] = 0;
303 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
304 }
305
setRotZ(float phi)306 void MatrixFP::setRotZ(float phi)
307 {
308 form[0][0] = (pp_int32 )(cos(phi)*65536.0f); form[0][1] = (pp_int32 )(-sin(phi)*65536.0f); form[0][2] = 0; form[0][3] = 0;
309 form[1][0] = (pp_int32 )(sin(phi)*65536.0f); form[1][1] = (pp_int32 )(cos(phi)*65536.0f); form[1][2] = 0; form[1][3] = 0;
310 form[2][0] = 0; form[2][1] = 0; form[2][2] = 65536; form[2][3] = 0;
311 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
312 }
313
setRotXYZ(float phiX,float phiY,float phiZ)314 void MatrixFP::setRotXYZ(float phiX,float phiY,float phiZ)
315 {
316 MatrixFP m1,m2,m3,mr;
317 m1.setRotX(phiX);
318 m2.setRotY(phiY);
319 m3.setRotZ(phiZ);
320 mr = m2*m1;
321 mr = mr*m3;
322 memcpy(&form,&mr.form,sizeof(form));
323 }
324
setScale(VectorFloat v)325 void MatrixFP::setScale(VectorFloat v)
326 {
327 form[0][0] = (pp_int32 )(v.x*65536.0f); form[0][1] = 0; form[0][2] = 0; form[0][3] = 0;
328 form[1][0] = 0; form[1][1] = (pp_int32 )(v.y*65536.0f); form[1][2] = 0; form[1][3] = 0;
329 form[2][0] = 0; form[2][1] = 0; form[2][2] = (pp_int32 )(v.z*65536.0f); form[2][3] = 0;
330 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
331 }
332
setScale(VectorFP v)333 void MatrixFP::setScale(VectorFP v)
334 {
335 form[0][0] = v.x; form[0][1] = 0; form[0][2] = 0; form[0][3] = 0;
336 form[1][0] = 0; form[1][1] = v.y; form[1][2] = 0; form[1][3] = 0;
337 form[2][0] = 0; form[2][1] = 0; form[2][2] = v.z; form[2][3] = 0;
338 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
339 }
340
setTranslate(VectorFP v)341 void MatrixFP::setTranslate(VectorFP v)
342 {
343 form[0][0] = 65536; form[0][1] = 0; form[0][2] = 0; form[0][3] = v.x;
344 form[1][0] = 0; form[1][1] = 65536; form[1][2] = 0; form[1][3] = v.y;
345 form[2][0] = 0; form[2][1] = 0; form[2][2] = 65536; form[2][3] = v.z;
346 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
347 }
348
setSubtract(VectorFP v)349 void MatrixFP::setSubtract(VectorFP v)
350 {
351 form[0][0] = 65536; form[0][1] = 0; form[0][2] = 0; form[0][3] = -v.x;
352 form[1][0] = 0; form[1][1] = 65536; form[1][2] = 0; form[1][3] = -v.y;
353 form[2][0] = 0; form[2][1] = 0; form[2][2] = 65536; form[2][3] = -v.z;
354 form[3][0] = 0; form[3][1] = 0; form[3][2] = 0; form[3][3] = 65536;
355 }
356
stripTranslation()357 void MatrixFP::stripTranslation()
358 {
359 form[0][3] = 0;
360 form[1][3] = 0;
361 form[2][3] = 0;
362 form[3][3] = 65536;
363
364 form[3][0] = 0;
365 form[3][1] = 0;
366 form[3][2] = 0;
367 }
operator *(MatrixFP & m1,MatrixFP & m2)368 MatrixFP operator* (MatrixFP &m1,MatrixFP &m2)
369 {
370 pp_int32 i,j,k;
371
372 MatrixFP mr;
373
374 for (i = 0; i < 4; i++)
375 for (j = 0; j < 4; j++)
376 {
377 mr.form[i][j] = 0;
378 for (k = 0; k < 4; k++)
379 mr.form[i][j] += fpmul(m1.form[i][k],m2.form[k][j]);
380 }
381
382 return mr;
383
384 }
385
operator *(MatrixFP & m,VectorFP & v)386 VectorFP operator* (MatrixFP &m,VectorFP &v)
387 {
388 VectorFP vr;
389
390 vr.x = fpmul(m.form[0][0],v.x)+fpmul(m.form[0][1],v.y)+fpmul(m.form[0][2],v.z)+m.form[0][3];
391 vr.y = fpmul(m.form[1][0],v.x)+fpmul(m.form[1][1],v.y)+fpmul(m.form[1][2],v.z)+m.form[1][3];
392 vr.z = fpmul(m.form[2][0],v.x)+fpmul(m.form[2][1],v.y)+fpmul(m.form[2][2],v.z)+m.form[2][3];
393
394 return vr;
395 }
396
397 ////////////////////////////////
398 // floating point quaternions //
399 ////////////////////////////////
normalize()400 void QuaternionFloat::normalize()
401 {
402 float len1 = (w*w)+(v.x*v.x)+(v.y*v.y)+(v.z*v.z);
403
404 len1 = 1.0f/(float)sqrt(len1);
405
406 w*=len1;
407 v.x*=len1;
408 v.y*=len1;
409 v.z*=len1;
410 }
411
convertToFixedPoint()412 QuaternionFP QuaternionFloat::convertToFixedPoint()
413 {
414 QuaternionFP q;
415
416 q.v = v.convertToFixedPoint();
417 q.w = (pp_int32 )(w*65536.0f);
418
419 return q;
420 }
421
operator *(QuaternionFloat q1,float s)422 QuaternionFloat operator *(QuaternionFloat q1,float s)
423 {
424 QuaternionFloat q;
425
426 q.w=s*q1.w;
427 q.v.x=s*q1.v.x;
428 q.v.y=s*q1.v.y;
429 q.v.z=s*q1.v.z;
430
431 return q;
432 };
433
operator +(QuaternionFloat q1,QuaternionFloat q2)434 QuaternionFloat operator +(QuaternionFloat q1,QuaternionFloat q2)
435 {
436 QuaternionFloat q;
437
438 q.w=q1.w+q2.w;
439 q.v.x=q1.v.x+q2.v.x;
440 q.v.y=q1.v.y+q2.v.y;
441 q.v.z=q1.v.z+q2.v.z;
442
443 return q;
444 };
445
operator -(QuaternionFloat q1,QuaternionFloat q2)446 QuaternionFloat operator -(QuaternionFloat q1,QuaternionFloat q2)
447 {
448 QuaternionFloat q;
449
450 q.w=q1.w-q2.w;
451 q.v.x=q1.v.x-q2.v.x;
452 q.v.y=q1.v.y-q2.v.y;
453 q.v.z=q1.v.z-q2.v.z;
454
455 return q;
456 }
457
operator *(QuaternionFloat q1,QuaternionFloat q2)458 QuaternionFloat operator *(QuaternionFloat q1,QuaternionFloat q2)
459 {
460 QuaternionFloat q;
461
462 q.w=(q1.w*q2.w)-(q1.v*q2.v);
463
464 q.v=(q1.w*q2.v)+(q2.w*q1.v)+(q1.v^q2.v);
465
466 return q;
467 }
468
quaternionToMatrixFP(QuaternionFloat q)469 MatrixFP quaternionToMatrixFP(QuaternionFloat q)
470 {
471 MatrixFP m;
472
473 m.form[0][0]=(pp_int32 )((1.0f-(2.0f*q.v.y*q.v.y)-(2.0f*q.v.z*q.v.z))*65536.0f);
474 m.form[0][1]=(pp_int32 )((2.0f*q.v.x*q.v.y+2.0f*q.w*q.v.z)*65536.0f);
475 m.form[0][2]=(pp_int32 )((2.0f*q.v.x*q.v.z-2.0f*q.w*q.v.y)*65536.0f);
476 m.form[0][3]=0;
477
478 m.form[1][0]=(pp_int32 )((2.0f*q.v.x*q.v.y-2.0f*q.w*q.v.z)*65536.0f);
479 m.form[1][1]=(pp_int32 )((1.0f-(2.0f*q.v.x*q.v.x)-(2.0f*q.v.z*q.v.z))*65536.0f);
480 m.form[1][2]=(pp_int32 )((2.0f*q.v.y*q.v.z+2.0f*q.w*q.v.x)*65536.0f);
481 m.form[1][3]=0;
482
483 m.form[2][0]=(pp_int32 )((2.0f*q.v.x*q.v.z+2.0f*q.w*q.v.y)*65536.0f);
484 m.form[2][1]=(pp_int32 )((2.0f*q.v.y*q.v.z-2.0f*q.w*q.v.x)*65536.0f);
485 m.form[2][2]=(pp_int32 )((1.0f-(2.0f*q.v.x*q.v.x)-(2.0f*q.v.y*q.v.y))*65536.0f);
486 m.form[2][3]=0;
487
488 m.form[3][0] = m.form[3][1] = m.form[3][2] = 0; m.form[3][3] = 65536;
489
490 return m;
491 }
492
493 /////////////////////////////
494 // Fixed Point Quaternions //
495 /////////////////////////////
operator *(QuaternionFP & q1,pp_int32 s)496 QuaternionFP operator *(QuaternionFP &q1,pp_int32 s)
497 {
498 QuaternionFP q;
499
500 q.w=fpmul(s,q1.w);
501 q.v.x=fpmul(s,q1.v.x);
502 q.v.y=fpmul(s,q1.v.y);
503 q.v.z=fpmul(s,q1.v.z);
504
505 return q;
506 };
507
operator +(QuaternionFP & q1,QuaternionFP & q2)508 QuaternionFP operator +(QuaternionFP &q1,QuaternionFP &q2)
509 {
510 QuaternionFP q;
511
512 q.w=q1.w+q2.w;
513 q.v.x=q1.v.x+q2.v.x;
514 q.v.y=q1.v.y+q2.v.y;
515 q.v.z=q1.v.z+q2.v.z;
516
517 return q;
518 };
519
operator -(QuaternionFP & q1,QuaternionFP & q2)520 QuaternionFP operator -(QuaternionFP &q1,QuaternionFP &q2)
521 {
522 QuaternionFP q;
523
524 q.w=q1.w-q2.w;
525 q.v.x=q1.v.x-q2.v.x;
526 q.v.y=q1.v.y-q2.v.y;
527 q.v.z=q1.v.z-q2.v.z;
528
529 return q;
530 }
531
operator *(QuaternionFP & q1,QuaternionFP & q2)532 QuaternionFP operator *(QuaternionFP &q1,QuaternionFP &q2)
533 {
534 QuaternionFP q;
535
536 q.w=fpmul(q1.w,q2.w)-q1.v*q2.v;
537
538 q.v=(q1.w*q2.v)+(q2.w*q1.v)+(q1.v^q2.v);
539
540 return q;
541 }
542
quaternionToMatrixFP(QuaternionFP & q)543 MatrixFP quaternionToMatrixFP(QuaternionFP &q)
544 {
545 MatrixFP m;
546
547 m.form[0][0]=(pp_int32 )((65536-(2*fpmul(q.v.y,q.v.y))-(2*fpmul(q.v.z,q.v.z))));
548 m.form[0][1]=(pp_int32 )((2*fpmul(q.v.x,q.v.y)+2*fpmul(q.w,q.v.z)));
549 m.form[0][2]=(pp_int32 )((2*fpmul(q.v.x,q.v.z)-2*fpmul(q.w,q.v.y)));
550 m.form[0][3]=0;
551
552 m.form[1][0]=(pp_int32 )((2*fpmul(q.v.x,q.v.y)-2*fpmul(q.w,q.v.z)));
553 m.form[1][1]=(pp_int32 )((65536-(2*fpmul(q.v.x,q.v.x))-(2*fpmul(q.v.z,q.v.z))));
554 m.form[1][2]=(pp_int32 )((2*fpmul(q.v.y,q.v.z)+2*fpmul(q.w,q.v.x)));
555 m.form[1][3]=0;
556
557 m.form[2][0]=(pp_int32 )((2*fpmul(q.v.x,q.v.z)+2*fpmul(q.w,q.v.y)));
558 m.form[2][1]=(pp_int32 )((2*fpmul(q.v.y,q.v.z)-2*fpmul(q.w,q.v.x)));
559 m.form[2][2]=(pp_int32 )((65536-(2*fpmul(q.v.x,q.v.x))-(2*fpmul(q.v.y,q.v.y))));
560 m.form[2][3]=0;
561
562 m.form[3][0] = m.form[3][1] = m.form[3][2] = 0; m.form[3][3] = 65536;
563
564 return m;
565 }
566