46 #include "PQP_Compile.h"
49 const PQP_REAL M_PI = (PQP_REAL)3.14159265359;
57 ({double __value, __arg = (x); \
58 asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
65 ({double __value, __arg = (x); \
66 asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
73 #define myfabs(x) ((x < 0) ? -x : x)
80 Mprintg(
const PQP_REAL M[3][3])
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]);
91 Mfprint(FILE *f,
const PQP_REAL M[3][3])
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]);
101 Mprint(
const PQP_REAL M[3][3])
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]);
111 Vprintg(
const PQP_REAL V[3])
113 printf(
"%g %g %g\n", V[0], V[1], V[2]);
118 Vfprint(FILE *f,
const PQP_REAL V[3])
120 fprintf(f,
"%g %g %g\n", V[0], V[1], V[2]);
125 Vprint(
const PQP_REAL V[3])
127 printf(
"%g %g %g\n", V[0], V[1], V[2]);
132 Midentity(PQP_REAL M[3][3])
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;
141 Videntity(PQP_REAL T[3])
143 T[0] = T[1] = T[2] = 0.0;
148 McM(PQP_REAL Mr[3][3],
const PQP_REAL M[3][3])
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];
157 MTcM(PQP_REAL Mr[3][3],
const PQP_REAL M[3][3])
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];
166 VcV(PQP_REAL Vr[3],
const PQP_REAL V[3])
168 Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2];
173 McolcV(PQP_REAL Vr[3],
const PQP_REAL M[3][3],
int c)
182 McolcMcol(PQP_REAL Mr[3][3],
int cr,
const PQP_REAL M[3][3],
int c)
191 MxMpV(PQP_REAL Mr[3][3],
const PQP_REAL M1[3][3],
const PQP_REAL M2[3][3],
const PQP_REAL T[3])
193 Mr[0][0] = (M1[0][0] * M2[0][0] +
194 M1[0][1] * M2[1][0] +
195 M1[0][2] * M2[2][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] +
201 Mr[2][0] = (M1[2][0] * M2[0][0] +
202 M1[2][1] * M2[1][0] +
203 M1[2][2] * M2[2][0] +
205 Mr[0][1] = (M1[0][0] * M2[0][1] +
206 M1[0][1] * M2[1][1] +
207 M1[0][2] * M2[2][1] +
209 Mr[1][1] = (M1[1][0] * M2[0][1] +
210 M1[1][1] * M2[1][1] +
211 M1[1][2] * M2[2][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] +
217 Mr[0][2] = (M1[0][0] * M2[0][2] +
218 M1[0][1] * M2[1][2] +
219 M1[0][2] * M2[2][2] +
221 Mr[1][2] = (M1[1][0] * M2[0][2] +
222 M1[1][1] * M2[1][2] +
223 M1[1][2] * M2[2][2] +
225 Mr[2][2] = (M1[2][0] * M2[0][2] +
226 M1[2][1] * M2[1][2] +
227 M1[2][2] * M2[2][2] +
233 MxM(PQP_REAL Mr[3][3],
const PQP_REAL M1[3][3],
const PQP_REAL M2[3][3])
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]);
267 MxMT(PQP_REAL Mr[3][3],
const PQP_REAL M1[3][3],
const PQP_REAL M2[3][3])
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]);
300 MTxM(PQP_REAL Mr[3][3],
const PQP_REAL M1[3][3],
const PQP_REAL M2[3][3])
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]);
333 MxV(PQP_REAL Vr[3],
const PQP_REAL M1[3][3],
const PQP_REAL V1[3])
335 Vr[0] = (M1[0][0] * V1[0] +
338 Vr[1] = (M1[1][0] * V1[0] +
341 Vr[2] = (M1[2][0] * V1[0] +
349 MxVpV(PQP_REAL Vr[3],
const PQP_REAL M1[3][3],
const PQP_REAL V1[3],
const PQP_REAL V2[3])
351 Vr[0] = (M1[0][0] * V1[0] +
355 Vr[1] = (M1[1][0] * V1[0] +
359 Vr[2] = (M1[2][0] * V1[0] +
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])
370 Vr[0] = s1 * (M1[0][0] * V1[0] +
374 Vr[1] = s1 * (M1[1][0] * V1[0] +
378 Vr[2] = s1 * (M1[2][0] * V1[0] +
386 MTxV(PQP_REAL Vr[3],
const PQP_REAL M1[3][3],
const PQP_REAL V1[3])
388 Vr[0] = (M1[0][0] * V1[0] +
391 Vr[1] = (M1[0][1] * V1[0] +
394 Vr[2] = (M1[0][2] * V1[0] +
401 sMTxV(PQP_REAL Vr[3], PQP_REAL s1,
const PQP_REAL M1[3][3],
const PQP_REAL V1[3])
403 Vr[0] = s1*(M1[0][0] * V1[0] +
406 Vr[1] = s1*(M1[0][1] * V1[0] +
409 Vr[2] = s1*(M1[0][2] * V1[0] +
416 sMxV(PQP_REAL Vr[3], PQP_REAL s1,
const PQP_REAL M1[3][3],
const PQP_REAL V1[3])
418 Vr[0] = s1*(M1[0][0] * V1[0] +
421 Vr[1] = s1*(M1[1][0] * V1[0] +
424 Vr[2] = s1*(M1[2][0] * V1[0] +
432 VmV(PQP_REAL Vr[3],
const PQP_REAL V1[3],
const PQP_REAL V2[3])
434 Vr[0] = V1[0] - V2[0];
435 Vr[1] = V1[1] - V2[1];
436 Vr[2] = V1[2] - V2[2];
441 VpV(PQP_REAL Vr[3],
const PQP_REAL V1[3],
const PQP_REAL V2[3])
443 Vr[0] = V1[0] + V2[0];
444 Vr[1] = V1[1] + V2[1];
445 Vr[2] = V1[2] + V2[2];
450 VpVxS(PQP_REAL Vr[3],
const PQP_REAL V1[3],
const PQP_REAL V2[3], PQP_REAL s)
452 Vr[0] = V1[0] + V2[0] * s;
453 Vr[1] = V1[1] + V2[1] * s;
454 Vr[2] = V1[2] + V2[2] * s;
459 MskewV(PQP_REAL M[3][3],
const PQP_REAL v[3])
461 M[0][0] = M[1][1] = M[2][2] = 0.0;
473 VcrossV(PQP_REAL Vr[3],
const PQP_REAL V1[3],
const PQP_REAL V2[3])
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];
482 Vlength(PQP_REAL V[3])
484 return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
489 Vnormalize(PQP_REAL V[3])
491 PQP_REAL d = (PQP_REAL)1.0 / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
499 VdotV(
const PQP_REAL V1[3],
const PQP_REAL V2[3])
501 return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
506 VdistV2(
const PQP_REAL V1[3],
const PQP_REAL V2[3])
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]));
515 VxS(PQP_REAL Vr[3],
const PQP_REAL V[3], PQP_REAL s)
524 MRotZ(PQP_REAL Mr[3][3], PQP_REAL t)
528 Mr[0][1] = -Mr[1][0];
530 Mr[2][0] = Mr[2][1] = 0.0;
531 Mr[0][2] = Mr[1][2] = 0.0;
537 MRotX(PQP_REAL Mr[3][3], PQP_REAL t)
541 Mr[1][2] = -Mr[2][1];
543 Mr[0][1] = Mr[0][2] = 0.0;
544 Mr[1][0] = Mr[2][0] = 0.0;
550 MRotY(PQP_REAL Mr[3][3], PQP_REAL t)
554 Mr[2][0] = -Mr[0][2];
556 Mr[1][2] = Mr[1][0] = 0.0;
557 Mr[2][1] = Mr[0][1] = 0.0;
563 MVtoOGL(
double oglm[16],
const PQP_REAL R[3][3],
const PQP_REAL T[3])
565 oglm[0] = (double)R[0][0];
566 oglm[1] = (double)R[1][0];
567 oglm[2] = (double)R[2][0];
569 oglm[4] = (double)R[0][1];
570 oglm[5] = (double)R[1][1];
571 oglm[6] = (double)R[2][1];
573 oglm[8] = (double)R[0][2];
574 oglm[9] = (double)R[1][2];
575 oglm[10] = (double)R[2][2];
577 oglm[12] = (double)T[0];
578 oglm[13] = (double)T[1];
579 oglm[14] = (double)T[2];
585 OGLtoMV(PQP_REAL R[3][3], PQP_REAL T[3],
const double oglm[16])
587 R[0][0] = (PQP_REAL)oglm[0];
588 R[1][0] = (PQP_REAL)oglm[1];
589 R[2][0] = (PQP_REAL)oglm[2];
591 R[0][1] = (PQP_REAL)oglm[4];
592 R[1][1] = (PQP_REAL)oglm[5];
593 R[2][1] = (PQP_REAL)oglm[6];
595 R[0][2] = (PQP_REAL)oglm[8];
596 R[1][2] = (PQP_REAL)oglm[9];
597 R[2][2] = (PQP_REAL)oglm[10];
599 T[0] = (PQP_REAL)oglm[12];
600 T[1] = (PQP_REAL)oglm[13];
601 T[2] = (PQP_REAL)oglm[14];
612 MRotQ(PQP_REAL destMatrix[3][3], PQP_REAL srcQuat[4])
625 s = (PQP_REAL)2.0 / (srcQuat[QX]*srcQuat[QX] + srcQuat[QY]*srcQuat[QY] +
626 srcQuat[QZ]*srcQuat[QZ] + srcQuat[QW]*srcQuat[QW]);
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;
633 destMatrix[QX][QX] = (PQP_REAL)1.0 - (yy + zz);
634 destMatrix[QX][QY] = xy + wz;
635 destMatrix[QX][QZ] = xz - wy;
637 destMatrix[QY][QX] = xy - wz;
638 destMatrix[QY][QY] = (PQP_REAL)1.0 - (xx + zz);
639 destMatrix[QY][QZ] = yz + wx;
641 destMatrix[QZ][QX] = xz + wy;
642 destMatrix[QZ][QY] = yz - wx;
643 destMatrix[QZ][QZ] = (PQP_REAL)1.0 - (xx + yy);
648 Mqinverse(PQP_REAL Mr[3][3], PQP_REAL m[3][3])
659 Mr[i][j] = (m[j1][i1]*m[j2][i2] - m[j1][i2]*m[j2][i1]);
667 #define rfabs(x) ((x < 0) ? -x : x)
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);
673 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
676 PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
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;
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;
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; }
702 if (i < 3) tresh=0.2*sm/(3*3);
else tresh=0.0;
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]))
708 else if (rfabs(a[0][1])>tresh)
711 if (rfabs(h)+g == rfabs(h)) t=(a[0][1])/h;
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;
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;
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);
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]))
730 else if (rfabs(a[0][2])>tresh)
733 if (rfabs(h)+g == rfabs(h)) t=(a[0][2])/h;
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;
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;
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);
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]))
753 else if (rfabs(a[1][2])>tresh)
756 if (rfabs(h)+g == rfabs(h)) t=(a[1][2])/h;
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;
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;
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);
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;
777 fprintf(stderr,
"eigen: too many iterations in Jacobi transform (%d).\n", i);
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);
790 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
794 PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
802 for(ip=0; ip<n; ip++)
815 for(ip=0;ip<n;ip++)
for(iq=ip+1;iq<n;iq++) sm+=fabs(a[ip][iq]);
824 if (i < 3) tresh=(PQP_REAL)0.2*sm/(n*n);
827 for(ip=0; ip<n; ip++)
for(iq=ip+1; iq<n; iq++)
829 g = (PQP_REAL)100.0*fabs(a[ip][iq]);
831 fabs(d[ip])+g==fabs(d[ip]) &&
832 fabs(d[iq])+g==fabs(d[iq]))
834 else if (fabs(a[ip][iq])>tresh)
837 if (fabs(h)+g == fabs(h)) t=(a[ip][iq])/h;
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;
844 c=(PQP_REAL)1.0/sqrt(1+t*t);
846 tau=s/((PQP_REAL)1.0+c);
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); }
868 fprintf(stderr,
"eigen: too many iterations in Jacobi transform.\n");