pqp
 All Classes
MatVec.h
1 /*************************************************************************\
2 
3  Copyright 1999 The University of North Carolina at Chapel Hill.
4  All Rights Reserved.
5 
6  Permission to use, copy, modify and distribute this software and its
7  documentation for educational, research and non-profit purposes, without
8  fee, and without a written agreement is hereby granted, provided that the
9  above copyright notice and the following three paragraphs appear in all
10  copies.
11 
12  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17  DAMAGES.
18 
19  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
20  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
22  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
23  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
24  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 
26  The authors may be contacted via:
27 
28  US Mail: S. Gottschalk
29  Department of Computer Science
30  Sitterson Hall, CB #3175
31  University of N. Carolina
32  Chapel Hill, NC 27599-3175
33 
34  Phone: (919)962-1749
35 
36  EMail: geom@cs.unc.edu
37 
38 
39 \**************************************************************************/
40 
41 #ifndef PQP_MATVEC_H
42 #define PQP_MATVEC_H
43 
44 #include <math.h>
45 #include <stdio.h>
46 #include "PQP_Compile.h"
47 
48 #ifndef M_PI
49 const PQP_REAL M_PI = (PQP_REAL)3.14159265359;
50 #endif
51 
52 #ifdef gnu
53 #include "zzzz.h"
54 
55 #ifdef hppa
56 #define myfabs(x) \
57  ({double __value, __arg = (x); \
58  asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
59  __value; \
60 });
61 #endif
62 
63 #ifdef mips
64 #define myfabs(x) \
65  ({double __value, __arg = (x); \
66  asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
67  __value; \
68 });
69 #endif
70 
71 #else
72 
73 #define myfabs(x) ((x < 0) ? -x : x)
74 
75 #endif
76 
77 
78 inline
79 void
80 Mprintg(const PQP_REAL M[3][3])
81 {
82  printf("%g %g %g\n%g %g %g\n%g %g %g\n",
83  M[0][0], M[0][1], M[0][2],
84  M[1][0], M[1][1], M[1][2],
85  M[2][0], M[2][1], M[2][2]);
86 }
87 
88 
89 inline
90 void
91 Mfprint(FILE *f, const PQP_REAL M[3][3])
92 {
93  fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n",
94  M[0][0], M[0][1], M[0][2],
95  M[1][0], M[1][1], M[1][2],
96  M[2][0], M[2][1], M[2][2]);
97 }
98 
99 inline
100 void
101 Mprint(const PQP_REAL M[3][3])
102 {
103  printf("%g %g %g\n%g %g %g\n%g %g %g\n",
104  M[0][0], M[0][1], M[0][2],
105  M[1][0], M[1][1], M[1][2],
106  M[2][0], M[2][1], M[2][2]);
107 }
108 
109 inline
110 void
111 Vprintg(const PQP_REAL V[3])
112 {
113  printf("%g %g %g\n", V[0], V[1], V[2]);
114 }
115 
116 inline
117 void
118 Vfprint(FILE *f, const PQP_REAL V[3])
119 {
120  fprintf(f, "%g %g %g\n", V[0], V[1], V[2]);
121 }
122 
123 inline
124 void
125 Vprint(const PQP_REAL V[3])
126 {
127  printf("%g %g %g\n", V[0], V[1], V[2]);
128 }
129 
130 inline
131 void
132 Midentity(PQP_REAL M[3][3])
133 {
134  M[0][0] = M[1][1] = M[2][2] = 1.0;
135  M[0][1] = M[1][2] = M[2][0] = 0.0;
136  M[0][2] = M[1][0] = M[2][1] = 0.0;
137 }
138 
139 inline
140 void
141 Videntity(PQP_REAL T[3])
142 {
143  T[0] = T[1] = T[2] = 0.0;
144 }
145 
146 inline
147 void
148 McM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
149 {
150  Mr[0][0] = M[0][0]; Mr[0][1] = M[0][1]; Mr[0][2] = M[0][2];
151  Mr[1][0] = M[1][0]; Mr[1][1] = M[1][1]; Mr[1][2] = M[1][2];
152  Mr[2][0] = M[2][0]; Mr[2][1] = M[2][1]; Mr[2][2] = M[2][2];
153 }
154 
155 inline
156 void
157 MTcM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
158 {
159  Mr[0][0] = M[0][0]; Mr[1][0] = M[0][1]; Mr[2][0] = M[0][2];
160  Mr[0][1] = M[1][0]; Mr[1][1] = M[1][1]; Mr[2][1] = M[1][2];
161  Mr[0][2] = M[2][0]; Mr[1][2] = M[2][1]; Mr[2][2] = M[2][2];
162 }
163 
164 inline
165 void
166 VcV(PQP_REAL Vr[3], const PQP_REAL V[3])
167 {
168  Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2];
169 }
170 
171 inline
172 void
173 McolcV(PQP_REAL Vr[3], const PQP_REAL M[3][3], int c)
174 {
175  Vr[0] = M[0][c];
176  Vr[1] = M[1][c];
177  Vr[2] = M[2][c];
178 }
179 
180 inline
181 void
182 McolcMcol(PQP_REAL Mr[3][3], int cr, const PQP_REAL M[3][3], int c)
183 {
184  Mr[0][cr] = M[0][c];
185  Mr[1][cr] = M[1][c];
186  Mr[2][cr] = M[2][c];
187 }
188 
189 inline
190 void
191 MxMpV(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3], const PQP_REAL T[3])
192 {
193  Mr[0][0] = (M1[0][0] * M2[0][0] +
194  M1[0][1] * M2[1][0] +
195  M1[0][2] * M2[2][0] +
196  T[0]);
197  Mr[1][0] = (M1[1][0] * M2[0][0] +
198  M1[1][1] * M2[1][0] +
199  M1[1][2] * M2[2][0] +
200  T[1]);
201  Mr[2][0] = (M1[2][0] * M2[0][0] +
202  M1[2][1] * M2[1][0] +
203  M1[2][2] * M2[2][0] +
204  T[2]);
205  Mr[0][1] = (M1[0][0] * M2[0][1] +
206  M1[0][1] * M2[1][1] +
207  M1[0][2] * M2[2][1] +
208  T[0]);
209  Mr[1][1] = (M1[1][0] * M2[0][1] +
210  M1[1][1] * M2[1][1] +
211  M1[1][2] * M2[2][1] +
212  T[1]);
213  Mr[2][1] = (M1[2][0] * M2[0][1] +
214  M1[2][1] * M2[1][1] +
215  M1[2][2] * M2[2][1] +
216  T[2]);
217  Mr[0][2] = (M1[0][0] * M2[0][2] +
218  M1[0][1] * M2[1][2] +
219  M1[0][2] * M2[2][2] +
220  T[0]);
221  Mr[1][2] = (M1[1][0] * M2[0][2] +
222  M1[1][1] * M2[1][2] +
223  M1[1][2] * M2[2][2] +
224  T[1]);
225  Mr[2][2] = (M1[2][0] * M2[0][2] +
226  M1[2][1] * M2[1][2] +
227  M1[2][2] * M2[2][2] +
228  T[2]);
229 }
230 
231 inline
232 void
233 MxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
234 {
235  Mr[0][0] = (M1[0][0] * M2[0][0] +
236  M1[0][1] * M2[1][0] +
237  M1[0][2] * M2[2][0]);
238  Mr[1][0] = (M1[1][0] * M2[0][0] +
239  M1[1][1] * M2[1][0] +
240  M1[1][2] * M2[2][0]);
241  Mr[2][0] = (M1[2][0] * M2[0][0] +
242  M1[2][1] * M2[1][0] +
243  M1[2][2] * M2[2][0]);
244  Mr[0][1] = (M1[0][0] * M2[0][1] +
245  M1[0][1] * M2[1][1] +
246  M1[0][2] * M2[2][1]);
247  Mr[1][1] = (M1[1][0] * M2[0][1] +
248  M1[1][1] * M2[1][1] +
249  M1[1][2] * M2[2][1]);
250  Mr[2][1] = (M1[2][0] * M2[0][1] +
251  M1[2][1] * M2[1][1] +
252  M1[2][2] * M2[2][1]);
253  Mr[0][2] = (M1[0][0] * M2[0][2] +
254  M1[0][1] * M2[1][2] +
255  M1[0][2] * M2[2][2]);
256  Mr[1][2] = (M1[1][0] * M2[0][2] +
257  M1[1][1] * M2[1][2] +
258  M1[1][2] * M2[2][2]);
259  Mr[2][2] = (M1[2][0] * M2[0][2] +
260  M1[2][1] * M2[1][2] +
261  M1[2][2] * M2[2][2]);
262 }
263 
264 
265 inline
266 void
267 MxMT(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
268 {
269  Mr[0][0] = (M1[0][0] * M2[0][0] +
270  M1[0][1] * M2[0][1] +
271  M1[0][2] * M2[0][2]);
272  Mr[1][0] = (M1[1][0] * M2[0][0] +
273  M1[1][1] * M2[0][1] +
274  M1[1][2] * M2[0][2]);
275  Mr[2][0] = (M1[2][0] * M2[0][0] +
276  M1[2][1] * M2[0][1] +
277  M1[2][2] * M2[0][2]);
278  Mr[0][1] = (M1[0][0] * M2[1][0] +
279  M1[0][1] * M2[1][1] +
280  M1[0][2] * M2[1][2]);
281  Mr[1][1] = (M1[1][0] * M2[1][0] +
282  M1[1][1] * M2[1][1] +
283  M1[1][2] * M2[1][2]);
284  Mr[2][1] = (M1[2][0] * M2[1][0] +
285  M1[2][1] * M2[1][1] +
286  M1[2][2] * M2[1][2]);
287  Mr[0][2] = (M1[0][0] * M2[2][0] +
288  M1[0][1] * M2[2][1] +
289  M1[0][2] * M2[2][2]);
290  Mr[1][2] = (M1[1][0] * M2[2][0] +
291  M1[1][1] * M2[2][1] +
292  M1[1][2] * M2[2][2]);
293  Mr[2][2] = (M1[2][0] * M2[2][0] +
294  M1[2][1] * M2[2][1] +
295  M1[2][2] * M2[2][2]);
296 }
297 
298 inline
299 void
300 MTxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
301 {
302  Mr[0][0] = (M1[0][0] * M2[0][0] +
303  M1[1][0] * M2[1][0] +
304  M1[2][0] * M2[2][0]);
305  Mr[1][0] = (M1[0][1] * M2[0][0] +
306  M1[1][1] * M2[1][0] +
307  M1[2][1] * M2[2][0]);
308  Mr[2][0] = (M1[0][2] * M2[0][0] +
309  M1[1][2] * M2[1][0] +
310  M1[2][2] * M2[2][0]);
311  Mr[0][1] = (M1[0][0] * M2[0][1] +
312  M1[1][0] * M2[1][1] +
313  M1[2][0] * M2[2][1]);
314  Mr[1][1] = (M1[0][1] * M2[0][1] +
315  M1[1][1] * M2[1][1] +
316  M1[2][1] * M2[2][1]);
317  Mr[2][1] = (M1[0][2] * M2[0][1] +
318  M1[1][2] * M2[1][1] +
319  M1[2][2] * M2[2][1]);
320  Mr[0][2] = (M1[0][0] * M2[0][2] +
321  M1[1][0] * M2[1][2] +
322  M1[2][0] * M2[2][2]);
323  Mr[1][2] = (M1[0][1] * M2[0][2] +
324  M1[1][1] * M2[1][2] +
325  M1[2][1] * M2[2][2]);
326  Mr[2][2] = (M1[0][2] * M2[0][2] +
327  M1[1][2] * M2[1][2] +
328  M1[2][2] * M2[2][2]);
329 }
330 
331 inline
332 void
333 MxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
334 {
335  Vr[0] = (M1[0][0] * V1[0] +
336  M1[0][1] * V1[1] +
337  M1[0][2] * V1[2]);
338  Vr[1] = (M1[1][0] * V1[0] +
339  M1[1][1] * V1[1] +
340  M1[1][2] * V1[2]);
341  Vr[2] = (M1[2][0] * V1[0] +
342  M1[2][1] * V1[1] +
343  M1[2][2] * V1[2]);
344 }
345 
346 
347 inline
348 void
349 MxVpV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
350 {
351  Vr[0] = (M1[0][0] * V1[0] +
352  M1[0][1] * V1[1] +
353  M1[0][2] * V1[2] +
354  V2[0]);
355  Vr[1] = (M1[1][0] * V1[0] +
356  M1[1][1] * V1[1] +
357  M1[1][2] * V1[2] +
358  V2[1]);
359  Vr[2] = (M1[2][0] * V1[0] +
360  M1[2][1] * V1[1] +
361  M1[2][2] * V1[2] +
362  V2[2]);
363 }
364 
365 
366 inline
367 void
368 sMxVpV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
369 {
370  Vr[0] = s1 * (M1[0][0] * V1[0] +
371  M1[0][1] * V1[1] +
372  M1[0][2] * V1[2]) +
373  V2[0];
374  Vr[1] = s1 * (M1[1][0] * V1[0] +
375  M1[1][1] * V1[1] +
376  M1[1][2] * V1[2]) +
377  V2[1];
378  Vr[2] = s1 * (M1[2][0] * V1[0] +
379  M1[2][1] * V1[1] +
380  M1[2][2] * V1[2]) +
381  V2[2];
382 }
383 
384 inline
385 void
386 MTxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
387 {
388  Vr[0] = (M1[0][0] * V1[0] +
389  M1[1][0] * V1[1] +
390  M1[2][0] * V1[2]);
391  Vr[1] = (M1[0][1] * V1[0] +
392  M1[1][1] * V1[1] +
393  M1[2][1] * V1[2]);
394  Vr[2] = (M1[0][2] * V1[0] +
395  M1[1][2] * V1[1] +
396  M1[2][2] * V1[2]);
397 }
398 
399 inline
400 void
401 sMTxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
402 {
403  Vr[0] = s1*(M1[0][0] * V1[0] +
404  M1[1][0] * V1[1] +
405  M1[2][0] * V1[2]);
406  Vr[1] = s1*(M1[0][1] * V1[0] +
407  M1[1][1] * V1[1] +
408  M1[2][1] * V1[2]);
409  Vr[2] = s1*(M1[0][2] * V1[0] +
410  M1[1][2] * V1[1] +
411  M1[2][2] * V1[2]);
412 }
413 
414 inline
415 void
416 sMxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
417 {
418  Vr[0] = s1*(M1[0][0] * V1[0] +
419  M1[0][1] * V1[1] +
420  M1[0][2] * V1[2]);
421  Vr[1] = s1*(M1[1][0] * V1[0] +
422  M1[1][1] * V1[1] +
423  M1[1][2] * V1[2]);
424  Vr[2] = s1*(M1[2][0] * V1[0] +
425  M1[2][1] * V1[1] +
426  M1[2][2] * V1[2]);
427 }
428 
429 
430 inline
431 void
432 VmV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
433 {
434  Vr[0] = V1[0] - V2[0];
435  Vr[1] = V1[1] - V2[1];
436  Vr[2] = V1[2] - V2[2];
437 }
438 
439 inline
440 void
441 VpV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
442 {
443  Vr[0] = V1[0] + V2[0];
444  Vr[1] = V1[1] + V2[1];
445  Vr[2] = V1[2] + V2[2];
446 }
447 
448 inline
449 void
450 VpVxS(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3], PQP_REAL s)
451 {
452  Vr[0] = V1[0] + V2[0] * s;
453  Vr[1] = V1[1] + V2[1] * s;
454  Vr[2] = V1[2] + V2[2] * s;
455 }
456 
457 inline
458 void
459 MskewV(PQP_REAL M[3][3], const PQP_REAL v[3])
460 {
461  M[0][0] = M[1][1] = M[2][2] = 0.0;
462  M[1][0] = v[2];
463  M[0][1] = -v[2];
464  M[0][2] = v[1];
465  M[2][0] = -v[1];
466  M[1][2] = -v[0];
467  M[2][1] = v[0];
468 }
469 
470 
471 inline
472 void
473 VcrossV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
474 {
475  Vr[0] = V1[1]*V2[2] - V1[2]*V2[1];
476  Vr[1] = V1[2]*V2[0] - V1[0]*V2[2];
477  Vr[2] = V1[0]*V2[1] - V1[1]*V2[0];
478 }
479 
480 inline
481 PQP_REAL
482 Vlength(PQP_REAL V[3])
483 {
484  return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
485 }
486 
487 inline
488 void
489 Vnormalize(PQP_REAL V[3])
490 {
491  PQP_REAL d = (PQP_REAL)1.0 / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
492  V[0] *= d;
493  V[1] *= d;
494  V[2] *= d;
495 }
496 
497 inline
498 PQP_REAL
499 VdotV(const PQP_REAL V1[3], const PQP_REAL V2[3])
500 {
501  return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
502 }
503 
504 inline
505 PQP_REAL
506 VdistV2(const PQP_REAL V1[3], const PQP_REAL V2[3])
507 {
508  return ( (V1[0]-V2[0]) * (V1[0]-V2[0]) +
509  (V1[1]-V2[1]) * (V1[1]-V2[1]) +
510  (V1[2]-V2[2]) * (V1[2]-V2[2]));
511 }
512 
513 inline
514 void
515 VxS(PQP_REAL Vr[3], const PQP_REAL V[3], PQP_REAL s)
516 {
517  Vr[0] = V[0] * s;
518  Vr[1] = V[1] * s;
519  Vr[2] = V[2] * s;
520 }
521 
522 inline
523 void
524 MRotZ(PQP_REAL Mr[3][3], PQP_REAL t)
525 {
526  Mr[0][0] = cos(t);
527  Mr[1][0] = sin(t);
528  Mr[0][1] = -Mr[1][0];
529  Mr[1][1] = Mr[0][0];
530  Mr[2][0] = Mr[2][1] = 0.0;
531  Mr[0][2] = Mr[1][2] = 0.0;
532  Mr[2][2] = 1.0;
533 }
534 
535 inline
536 void
537 MRotX(PQP_REAL Mr[3][3], PQP_REAL t)
538 {
539  Mr[1][1] = cos(t);
540  Mr[2][1] = sin(t);
541  Mr[1][2] = -Mr[2][1];
542  Mr[2][2] = Mr[1][1];
543  Mr[0][1] = Mr[0][2] = 0.0;
544  Mr[1][0] = Mr[2][0] = 0.0;
545  Mr[0][0] = 1.0;
546 }
547 
548 inline
549 void
550 MRotY(PQP_REAL Mr[3][3], PQP_REAL t)
551 {
552  Mr[2][2] = cos(t);
553  Mr[0][2] = sin(t);
554  Mr[2][0] = -Mr[0][2];
555  Mr[0][0] = Mr[2][2];
556  Mr[1][2] = Mr[1][0] = 0.0;
557  Mr[2][1] = Mr[0][1] = 0.0;
558  Mr[1][1] = 1.0;
559 }
560 
561 inline
562 void
563 MVtoOGL(double oglm[16], const PQP_REAL R[3][3], const PQP_REAL T[3])
564 {
565  oglm[0] = (double)R[0][0];
566  oglm[1] = (double)R[1][0];
567  oglm[2] = (double)R[2][0];
568  oglm[3] = 0.0;
569  oglm[4] = (double)R[0][1];
570  oglm[5] = (double)R[1][1];
571  oglm[6] = (double)R[2][1];
572  oglm[7] = 0.0;
573  oglm[8] = (double)R[0][2];
574  oglm[9] = (double)R[1][2];
575  oglm[10] = (double)R[2][2];
576  oglm[11] = 0.0;
577  oglm[12] = (double)T[0];
578  oglm[13] = (double)T[1];
579  oglm[14] = (double)T[2];
580  oglm[15] = 1.0;
581 }
582 
583 inline
584 void
585 OGLtoMV(PQP_REAL R[3][3], PQP_REAL T[3], const double oglm[16])
586 {
587  R[0][0] = (PQP_REAL)oglm[0];
588  R[1][0] = (PQP_REAL)oglm[1];
589  R[2][0] = (PQP_REAL)oglm[2];
590 
591  R[0][1] = (PQP_REAL)oglm[4];
592  R[1][1] = (PQP_REAL)oglm[5];
593  R[2][1] = (PQP_REAL)oglm[6];
594 
595  R[0][2] = (PQP_REAL)oglm[8];
596  R[1][2] = (PQP_REAL)oglm[9];
597  R[2][2] = (PQP_REAL)oglm[10];
598 
599  T[0] = (PQP_REAL)oglm[12];
600  T[1] = (PQP_REAL)oglm[13];
601  T[2] = (PQP_REAL)oglm[14];
602 }
603 
604 // taken from quatlib, written by Richard Holloway
605 const int QX = 0;
606 const int QY = 1;
607 const int QZ = 2;
608 const int QW = 3;
609 
610 inline
611 void
612 MRotQ(PQP_REAL destMatrix[3][3], PQP_REAL srcQuat[4])
613 {
614  PQP_REAL s;
615  PQP_REAL xs, ys, zs,
616  wx, wy, wz,
617  xx, xy, xz,
618  yy, yz, zz;
619 
620  /*
621  * For unit srcQuat, just set s = 2.0; or set xs = srcQuat[QX] +
622  * srcQuat[QX], etc.
623  */
624 
625  s = (PQP_REAL)2.0 / (srcQuat[QX]*srcQuat[QX] + srcQuat[QY]*srcQuat[QY] +
626  srcQuat[QZ]*srcQuat[QZ] + srcQuat[QW]*srcQuat[QW]);
627 
628  xs = srcQuat[QX] * s; ys = srcQuat[QY] * s; zs = srcQuat[QZ] * s;
629  wx = srcQuat[QW] * xs; wy = srcQuat[QW] * ys; wz = srcQuat[QW] * zs;
630  xx = srcQuat[QX] * xs; xy = srcQuat[QX] * ys; xz = srcQuat[QX] * zs;
631  yy = srcQuat[QY] * ys; yz = srcQuat[QY] * zs; zz = srcQuat[QZ] * zs;
632 
633  destMatrix[QX][QX] = (PQP_REAL)1.0 - (yy + zz);
634  destMatrix[QX][QY] = xy + wz;
635  destMatrix[QX][QZ] = xz - wy;
636 
637  destMatrix[QY][QX] = xy - wz;
638  destMatrix[QY][QY] = (PQP_REAL)1.0 - (xx + zz);
639  destMatrix[QY][QZ] = yz + wx;
640 
641  destMatrix[QZ][QX] = xz + wy;
642  destMatrix[QZ][QY] = yz - wx;
643  destMatrix[QZ][QZ] = (PQP_REAL)1.0 - (xx + yy);
644 }
645 
646 inline
647 void
648 Mqinverse(PQP_REAL Mr[3][3], PQP_REAL m[3][3])
649 {
650  int i,j;
651 
652  for(i=0; i<3; i++)
653  for(j=0; j<3; j++)
654  {
655  int i1 = (i+1)%3;
656  int i2 = (i+2)%3;
657  int j1 = (j+1)%3;
658  int j2 = (j+2)%3;
659  Mr[i][j] = (m[j1][i1]*m[j2][i2] - m[j1][i2]*m[j2][i1]);
660  }
661 }
662 
663 // Meigen from Numerical Recipes in C
664 
665 #if 0
666 
667 #define rfabs(x) ((x < 0) ? -x : x)
668 
669 #define ROT(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
670 
671 int
672 inline
673 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
674 {
675  int i;
676  PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
677  int nrot;
678  PQP_REAL b[3];
679  PQP_REAL z[3];
680  PQP_REAL v[3][3];
681  PQP_REAL d[3];
682 
683  v[0][0] = v[1][1] = v[2][2] = 1.0;
684  v[0][1] = v[1][2] = v[2][0] = 0.0;
685  v[0][2] = v[1][0] = v[2][1] = 0.0;
686 
687  b[0] = a[0][0]; d[0] = a[0][0]; z[0] = 0.0;
688  b[1] = a[1][1]; d[1] = a[1][1]; z[1] = 0.0;
689  b[2] = a[2][2]; d[2] = a[2][2]; z[2] = 0.0;
690 
691  nrot = 0;
692 
693 
694  for(i=0; i<50; i++)
695  {
696 
697  printf("2\n");
698 
699  sm=0.0; sm+=fabs(a[0][1]); sm+=fabs(a[0][2]); sm+=fabs(a[1][2]);
700  if (sm == 0.0) { McM(vout,v); VcV(dout,d); return i; }
701 
702  if (i < 3) tresh=0.2*sm/(3*3); else tresh=0.0;
703 
704  {
705  g = 100.0*rfabs(a[0][1]);
706  if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[1])+g==rfabs(d[1]))
707  a[0][1]=0.0;
708  else if (rfabs(a[0][1])>tresh)
709  {
710  h = d[1]-d[0];
711  if (rfabs(h)+g == rfabs(h)) t=(a[0][1])/h;
712  else
713  {
714  theta=0.5*h/(a[0][1]);
715  t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
716  if (theta < 0.0) t = -t;
717  }
718  c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][1];
719  z[0] -= h; z[1] += h; d[0] -= h; d[1] += h;
720  a[0][1]=0.0;
721  ROT(a,0,2,1,2); ROT(v,0,0,0,1); ROT(v,1,0,1,1); ROT(v,2,0,2,1);
722  nrot++;
723  }
724  }
725 
726  {
727  g = 100.0*rfabs(a[0][2]);
728  if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[2])+g==rfabs(d[2]))
729  a[0][2]=0.0;
730  else if (rfabs(a[0][2])>tresh)
731  {
732  h = d[2]-d[0];
733  if (rfabs(h)+g == rfabs(h)) t=(a[0][2])/h;
734  else
735  {
736  theta=0.5*h/(a[0][2]);
737  t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
738  if (theta < 0.0) t = -t;
739  }
740  c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][2];
741  z[0] -= h; z[2] += h; d[0] -= h; d[2] += h;
742  a[0][2]=0.0;
743  ROT(a,0,1,1,2); ROT(v,0,0,0,2); ROT(v,1,0,1,2); ROT(v,2,0,2,2);
744  nrot++;
745  }
746  }
747 
748 
749  {
750  g = 100.0*rfabs(a[1][2]);
751  if (i>3 && rfabs(d[1])+g==rfabs(d[1]) && rfabs(d[2])+g==rfabs(d[2]))
752  a[1][2]=0.0;
753  else if (rfabs(a[1][2])>tresh)
754  {
755  h = d[2]-d[1];
756  if (rfabs(h)+g == rfabs(h)) t=(a[1][2])/h;
757  else
758  {
759  theta=0.5*h/(a[1][2]);
760  t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
761  if (theta < 0.0) t = -t;
762  }
763  c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[1][2];
764  z[1] -= h; z[2] += h; d[1] -= h; d[2] += h;
765  a[1][2]=0.0;
766  ROT(a,0,1,0,2); ROT(v,0,1,0,2); ROT(v,1,1,1,2); ROT(v,2,1,2,2);
767  nrot++;
768  }
769  }
770 
771  b[0] += z[0]; d[0] = b[0]; z[0] = 0.0;
772  b[1] += z[1]; d[1] = b[1]; z[1] = 0.0;
773  b[2] += z[2]; d[2] = b[2]; z[2] = 0.0;
774 
775  }
776 
777  fprintf(stderr, "eigen: too many iterations in Jacobi transform (%d).\n", i);
778 
779  return i;
780 }
781 
782 #else
783 
784 
785 
786 #define ROTATE(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
787 
788 void
789 inline
790 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
791 {
792  int n = 3;
793  int j,iq,ip,i;
794  PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
795  int nrot;
796  PQP_REAL b[3];
797  PQP_REAL z[3];
798  PQP_REAL v[3][3];
799  PQP_REAL d[3];
800 
801  Midentity(v);
802  for(ip=0; ip<n; ip++)
803  {
804  b[ip] = a[ip][ip];
805  d[ip] = a[ip][ip];
806  z[ip] = 0.0;
807  }
808 
809  nrot = 0;
810 
811  for(i=0; i<50; i++)
812  {
813 
814  sm=0.0;
815  for(ip=0;ip<n;ip++) for(iq=ip+1;iq<n;iq++) sm+=fabs(a[ip][iq]);
816  if (sm == 0.0)
817  {
818  McM(vout, v);
819  VcV(dout, d);
820  return;
821  }
822 
823 
824  if (i < 3) tresh=(PQP_REAL)0.2*sm/(n*n);
825  else tresh=0.0;
826 
827  for(ip=0; ip<n; ip++) for(iq=ip+1; iq<n; iq++)
828  {
829  g = (PQP_REAL)100.0*fabs(a[ip][iq]);
830  if (i>3 &&
831  fabs(d[ip])+g==fabs(d[ip]) &&
832  fabs(d[iq])+g==fabs(d[iq]))
833  a[ip][iq]=0.0;
834  else if (fabs(a[ip][iq])>tresh)
835  {
836  h = d[iq]-d[ip];
837  if (fabs(h)+g == fabs(h)) t=(a[ip][iq])/h;
838  else
839  {
840  theta=(PQP_REAL)0.5*h/(a[ip][iq]);
841  t=(PQP_REAL)(1.0/(fabs(theta)+sqrt(1.0+theta*theta)));
842  if (theta < 0.0) t = -t;
843  }
844  c=(PQP_REAL)1.0/sqrt(1+t*t);
845  s=t*c;
846  tau=s/((PQP_REAL)1.0+c);
847  h=t*a[ip][iq];
848  z[ip] -= h;
849  z[iq] += h;
850  d[ip] -= h;
851  d[iq] += h;
852  a[ip][iq]=0.0;
853  for(j=0;j<ip;j++) { ROTATE(a,j,ip,j,iq); }
854  for(j=ip+1;j<iq;j++) { ROTATE(a,ip,j,j,iq); }
855  for(j=iq+1;j<n;j++) { ROTATE(a,ip,j,iq,j); }
856  for(j=0;j<n;j++) { ROTATE(v,j,ip,j,iq); }
857  nrot++;
858  }
859  }
860  for(ip=0;ip<n;ip++)
861  {
862  b[ip] += z[ip];
863  d[ip] = b[ip];
864  z[ip] = 0.0;
865  }
866  }
867 
868  fprintf(stderr, "eigen: too many iterations in Jacobi transform.\n");
869 
870  return;
871 }
872 
873 
874 #endif
875 
876 #endif
877 // MATVEC_H