///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// /// /// FICHIER : matvec.h /// /// /// /// NATURE : Matrix operation for managing collision /// /// /// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #ifndef MATVEC_H #define MATVEC_H #include #include // If GNU defined #ifdef gnu #include "zzzz.h" #ifdef hppa #define myfabs(x) \ ({float __value, __arg = (x); \ asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \ __value; \ }); #endif #ifdef mips #define myfabs(x) \ ({float __value, __arg = (x); \ asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \ __value; \ }); #endif // else if GNU not defined #else #define myfabs(x) ((x < 0) ? -x : x) #endif /////////////////////////////////////////////////////////////////////////// /// Mprintg /// /////////////////////////////////////////////////////////////////////////// inline void Mprintg(float M[3][3]) { printf("%g %g %g\n%g %g %g\n%g %g %g\n", M[0][0], M[0][1], M[0][2], M[1][0], M[1][1], M[1][2], M[2][0], M[2][1], M[2][2]); } /////////////////////////////////////////////////////////////////////////// /// Mfprint /// /////////////////////////////////////////////////////////////////////////// inline void Mfprint(FILE *f, float M[3][3]) { fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n", M[0][0], M[0][1], M[0][2], M[1][0], M[1][1], M[1][2], M[2][0], M[2][1], M[2][2]); } /////////////////////////////////////////////////////////////////////////// /// Mprint /// /////////////////////////////////////////////////////////////////////////// inline void Mprint(float M[3][3]) { printf("%g %g %g\n%g %g %g\n%g %g %g\n", M[0][0], M[0][1], M[0][2], M[1][0], M[1][1], M[1][2], M[2][0], M[2][1], M[2][2]); } /////////////////////////////////////////////////////////////////////////// /// Vprintg /// /////////////////////////////////////////////////////////////////////////// inline void Vprintg(float V[3]) { printf("%g %g %g\n", V[0], V[1], V[2]); } /////////////////////////////////////////////////////////////////////////// /// Vfprint /// /////////////////////////////////////////////////////////////////////////// inline void Vfprint(FILE *f, float V[3]) { fprintf(f, "%g %g %g\n", V[0], V[1], V[2]); } /////////////////////////////////////////////////////////////////////////// /// Vprint /// /////////////////////////////////////////////////////////////////////////// inline void Vprint(float V[3]) { printf("%g %g %g\n", V[0], V[1], V[2]); } /////////////////////////////////////////////////////////////////////////// /// Midentity /// /////////////////////////////////////////////////////////////////////////// inline void Midentity(float M[3][3]) { M[0][0] = M[1][1] = M[2][2] = 1.0; M[0][1] = M[1][2] = M[2][0] = 0.0; M[0][2] = M[1][0] = M[2][1] = 0.0; } /////////////////////////////////////////////////////////////////////////// /// McM /// /////////////////////////////////////////////////////////////////////////// inline void McM(float Mr[3][3], float M[3][3]) { Mr[0][0] = M[0][0]; Mr[0][1] = M[0][1]; Mr[0][2] = M[0][2]; Mr[1][0] = M[1][0]; Mr[1][1] = M[1][1]; Mr[1][2] = M[1][2]; Mr[2][0] = M[2][0]; Mr[2][1] = M[2][1]; Mr[2][2] = M[2][2]; } /////////////////////////////////////////////////////////////////////////// /// VcV /// /////////////////////////////////////////////////////////////////////////// inline void VcV(float Vr[3], float V[3]) { Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2]; } /////////////////////////////////////////////////////////////////////////// /// McolcV /// /////////////////////////////////////////////////////////////////////////// inline void McolcV(float Vr[3], float M[3][3], int c) { Vr[0] = M[0][c]; Vr[1] = M[1][c]; Vr[2] = M[2][c]; } /////////////////////////////////////////////////////////////////////////// /// McolcMcol /// /////////////////////////////////////////////////////////////////////////// inline void McolcMcol(float Mr[3][3], int cr, float M[3][3], int c) { Mr[0][cr] = M[0][c]; Mr[1][cr] = M[1][c]; Mr[2][cr] = M[2][c]; } /////////////////////////////////////////////////////////////////////////// /// MxMpV /// /////////////////////////////////////////////////////////////////////////// inline void MxMpV(float Mr[3][3], float M1[3][3], float M2[3][3], float T[3]) { Mr[0][0] = (M1[0][0] * M2[0][0] + M1[0][1] * M2[1][0] + M1[0][2] * M2[2][0] + T[0]); Mr[1][0] = (M1[1][0] * M2[0][0] + M1[1][1] * M2[1][0] + M1[1][2] * M2[2][0] + T[1]); Mr[2][0] = (M1[2][0] * M2[0][0] + M1[2][1] * M2[1][0] + M1[2][2] * M2[2][0] + T[2]); Mr[0][1] = (M1[0][0] * M2[0][1] + M1[0][1] * M2[1][1] + M1[0][2] * M2[2][1] + T[0]); Mr[1][1] = (M1[1][0] * M2[0][1] + M1[1][1] * M2[1][1] + M1[1][2] * M2[2][1] + T[1]); Mr[2][1] = (M1[2][0] * M2[0][1] + M1[2][1] * M2[1][1] + M1[2][2] * M2[2][1] + T[2]); Mr[0][2] = (M1[0][0] * M2[0][2] + M1[0][1] * M2[1][2] + M1[0][2] * M2[2][2] + T[0]); Mr[1][2] = (M1[1][0] * M2[0][2] + M1[1][1] * M2[1][2] + M1[1][2] * M2[2][2] + T[1]); Mr[2][2] = (M1[2][0] * M2[0][2] + M1[2][1] * M2[1][2] + M1[2][2] * M2[2][2] + T[2]); } /////////////////////////////////////////////////////////////////////////// /// MxM /// /////////////////////////////////////////////////////////////////////////// inline void MxM(float Mr[3][3], float M1[3][3], float M2[3][3]) { Mr[0][0] = (M1[0][0] * M2[0][0] + M1[0][1] * M2[1][0] + M1[0][2] * M2[2][0]); Mr[1][0] = (M1[1][0] * M2[0][0] + M1[1][1] * M2[1][0] + M1[1][2] * M2[2][0]); Mr[2][0] = (M1[2][0] * M2[0][0] + M1[2][1] * M2[1][0] + M1[2][2] * M2[2][0]); Mr[0][1] = (M1[0][0] * M2[0][1] + M1[0][1] * M2[1][1] + M1[0][2] * M2[2][1]); Mr[1][1] = (M1[1][0] * M2[0][1] + M1[1][1] * M2[1][1] + M1[1][2] * M2[2][1]); Mr[2][1] = (M1[2][0] * M2[0][1] + M1[2][1] * M2[1][1] + M1[2][2] * M2[2][1]); Mr[0][2] = (M1[0][0] * M2[0][2] + M1[0][1] * M2[1][2] + M1[0][2] * M2[2][2]); Mr[1][2] = (M1[1][0] * M2[0][2] + M1[1][1] * M2[1][2] + M1[1][2] * M2[2][2]); Mr[2][2] = (M1[2][0] * M2[0][2] + M1[2][1] * M2[1][2] + M1[2][2] * M2[2][2]); } /////////////////////////////////////////////////////////////////////////// /// MxMT /// /////////////////////////////////////////////////////////////////////////// inline void MxMT(float Mr[3][3], float M1[3][3], float M2[3][3]) { Mr[0][0] = (M1[0][0] * M2[0][0] + M1[0][1] * M2[0][1] + M1[0][2] * M2[0][2]); Mr[1][0] = (M1[1][0] * M2[0][0] + M1[1][1] * M2[0][1] + M1[1][2] * M2[0][2]); Mr[2][0] = (M1[2][0] * M2[0][0] + M1[2][1] * M2[0][1] + M1[2][2] * M2[0][2]); Mr[0][1] = (M1[0][0] * M2[1][0] + M1[0][1] * M2[1][1] + M1[0][2] * M2[1][2]); Mr[1][1] = (M1[1][0] * M2[1][0] + M1[1][1] * M2[1][1] + M1[1][2] * M2[1][2]); Mr[2][1] = (M1[2][0] * M2[1][0] + M1[2][1] * M2[1][1] + M1[2][2] * M2[1][2]); Mr[0][2] = (M1[0][0] * M2[2][0] + M1[0][1] * M2[2][1] + M1[0][2] * M2[2][2]); Mr[1][2] = (M1[1][0] * M2[2][0] + M1[1][1] * M2[2][1] + M1[1][2] * M2[2][2]); Mr[2][2] = (M1[2][0] * M2[2][0] + M1[2][1] * M2[2][1] + M1[2][2] * M2[2][2]); } /////////////////////////////////////////////////////////////////////////// /// MTxM /// /////////////////////////////////////////////////////////////////////////// inline void MTxM(float Mr[3][3], float M1[3][3], float M2[3][3]) { Mr[0][0] = (M1[0][0] * M2[0][0] + M1[1][0] * M2[1][0] + M1[2][0] * M2[2][0]); Mr[1][0] = (M1[0][1] * M2[0][0] + M1[1][1] * M2[1][0] + M1[2][1] * M2[2][0]); Mr[2][0] = (M1[0][2] * M2[0][0] + M1[1][2] * M2[1][0] + M1[2][2] * M2[2][0]); Mr[0][1] = (M1[0][0] * M2[0][1] + M1[1][0] * M2[1][1] + M1[2][0] * M2[2][1]); Mr[1][1] = (M1[0][1] * M2[0][1] + M1[1][1] * M2[1][1] + M1[2][1] * M2[2][1]); Mr[2][1] = (M1[0][2] * M2[0][1] + M1[1][2] * M2[1][1] + M1[2][2] * M2[2][1]); Mr[0][2] = (M1[0][0] * M2[0][2] + M1[1][0] * M2[1][2] + M1[2][0] * M2[2][2]); Mr[1][2] = (M1[0][1] * M2[0][2] + M1[1][1] * M2[1][2] + M1[2][1] * M2[2][2]); Mr[2][2] = (M1[0][2] * M2[0][2] + M1[1][2] * M2[1][2] + M1[2][2] * M2[2][2]); } /////////////////////////////////////////////////////////////////////////// /// MxV /// /////////////////////////////////////////////////////////////////////////// inline void MxV(float Vr[3], float M1[3][3], float V1[3]) { Vr[0] = (M1[0][0] * V1[0] + M1[0][1] * V1[1] + M1[0][2] * V1[2]); Vr[1] = (M1[1][0] * V1[0] + M1[1][1] * V1[1] + M1[1][2] * V1[2]); Vr[2] = (M1[2][0] * V1[0] + M1[2][1] * V1[1] + M1[2][2] * V1[2]); } /////////////////////////////////////////////////////////////////////////// /// MxVpV /// /////////////////////////////////////////////////////////////////////////// inline void MxVpV(float Vr[3], float M1[3][3], float V1[3], float V2[3]) { Vr[0] = (M1[0][0] * V1[0] + M1[0][1] * V1[1] + M1[0][2] * V1[2] + V2[0]); Vr[1] = (M1[1][0] * V1[0] + M1[1][1] * V1[1] + M1[1][2] * V1[2] + V2[1]); Vr[2] = (M1[2][0] * V1[0] + M1[2][1] * V1[1] + M1[2][2] * V1[2] + V2[2]); } /////////////////////////////////////////////////////////////////////////// /// sMxVpV /// /////////////////////////////////////////////////////////////////////////// inline void sMxVpV(float Vr[3], float s1, float M1[3][3], float V1[3], float V2[3]) { Vr[0] = s1 * (M1[0][0] * V1[0] + M1[0][1] * V1[1] + M1[0][2] * V1[2]) + V2[0]; Vr[1] = s1 * (M1[1][0] * V1[0] + M1[1][1] * V1[1] + M1[1][2] * V1[2]) + V2[1]; Vr[2] = s1 * (M1[2][0] * V1[0] + M1[2][1] * V1[1] + M1[2][2] * V1[2]) + V2[2]; } /////////////////////////////////////////////////////////////////////////// /// sMxV /// /////////////////////////////////////////////////////////////////////////// inline void sMxV(float Vr[3], float s1, float M1[3][3], float V1[3]) { Vr[0] = s1 * (M1[0][0] * V1[0] + M1[0][1] * V1[1] + M1[0][2] * V1[2]); Vr[1] = s1 * (M1[1][0] * V1[0] + M1[1][1] * V1[1] + M1[1][2] * V1[2]); Vr[2] = s1 * (M1[2][0] * V1[0] + M1[2][1] * V1[1] + M1[2][2] * V1[2]); } /////////////////////////////////////////////////////////////////////////// /// MTxV /// /////////////////////////////////////////////////////////////////////////// inline void MTxV(float Vr[3], float M1[3][3], float V1[3]) { Vr[0] = (M1[0][0] * V1[0] + M1[1][0] * V1[1] + M1[2][0] * V1[2]); Vr[1] = (M1[0][1] * V1[0] + M1[1][1] * V1[1] + M1[2][1] * V1[2]); Vr[2] = (M1[0][2] * V1[0] + M1[1][2] * V1[1] + M1[2][2] * V1[2]); } /////////////////////////////////////////////////////////////////////////// /// sMTxV /// /////////////////////////////////////////////////////////////////////////// inline void sMTxV(float Vr[3], float s1, float M1[3][3], float V1[3]) { Vr[0] = s1*(M1[0][0] * V1[0] + M1[1][0] * V1[1] + M1[2][0] * V1[2]); Vr[1] = s1*(M1[0][1] * V1[0] + M1[1][1] * V1[1] + M1[2][1] * V1[2]); Vr[2] = s1*(M1[0][2] * V1[0] + M1[1][2] * V1[1] + M1[2][2] * V1[2]); } /////////////////////////////////////////////////////////////////////////// /// VmV /// /////////////////////////////////////////////////////////////////////////// inline void VmV(float Vr[3], const float V1[3], const float V2[3]) { Vr[0] = V1[0] - V2[0]; Vr[1] = V1[1] - V2[1]; Vr[2] = V1[2] - V2[2]; } /////////////////////////////////////////////////////////////////////////// /// VpV /// /////////////////////////////////////////////////////////////////////////// inline void VpV(float Vr[3], float V1[3], float V2[3]) { Vr[0] = V1[0] + V2[0]; Vr[1] = V1[1] + V2[1]; Vr[2] = V1[2] + V2[2]; } /////////////////////////////////////////////////////////////////////////// /// VpVxS /// /////////////////////////////////////////////////////////////////////////// inline void VpVxS(float Vr[3], float V1[3], float V2[3], float s) { Vr[0] = V1[0] + V2[0] * s; Vr[1] = V1[1] + V2[1] * s; Vr[2] = V1[2] + V2[2] * s; } /////////////////////////////////////////////////////////////////////////// /// MskewV /// /////////////////////////////////////////////////////////////////////////// inline void MskewV(float M[3][3], const float v[3]) { M[0][0] = M[1][1] = M[2][2] = 0.0; M[1][0] = v[2]; M[0][1] = -v[2]; M[0][2] = v[1]; M[2][0] = -v[1]; M[1][2] = -v[0]; M[2][1] = v[0]; } /////////////////////////////////////////////////////////////////////////// /// VcrossV /// /////////////////////////////////////////////////////////////////////////// inline void VcrossV(float Vr[3], const float V1[3], const float V2[3]) { Vr[0] = V1[1]*V2[2] - V1[2]*V2[1]; Vr[1] = V1[2]*V2[0] - V1[0]*V2[2]; Vr[2] = V1[0]*V2[1] - V1[1]*V2[0]; } /////////////////////////////////////////////////////////////////////////// /// Vlength /// /////////////////////////////////////////////////////////////////////////// inline float Vlength(float V[3]) { return (float) sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]); } /////////////////////////////////////////////////////////////////////////// /// Vnormalize /// /////////////////////////////////////////////////////////////////////////// inline void Vnormalize(float V[3]) { float d = 1.0f / (float)sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]); V[0] *= d; V[1] *= d; V[2] *= d; } /////////////////////////////////////////////////////////////////////////// /// VdotV /// /////////////////////////////////////////////////////////////////////////// inline float VdotV(float V1[3], float V2[3]) { return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]); } /////////////////////////////////////////////////////////////////////////// /// VxS /// /////////////////////////////////////////////////////////////////////////// inline void VxS(float Vr[3], float V[3], float s) { Vr[0] = V[0] * s; Vr[1] = V[1] * s; Vr[2] = V[2] * s; } /////////////////////////////////////////////////////////////////////////// /// Mqinverse /// /////////////////////////////////////////////////////////////////////////// inline void Mqinverse(float Mr[3][3], float m[3][3]) { int i,j; for(i=0; i<3; i++) { for(j=0; j<3; j++) { int i1 = (i+1)%3; int i2 = (i+2)%3; int j1 = (j+1)%3; int j2 = (j+2)%3; Mr[i][j] = (m[j1][i1]*m[j2][i2] - m[j1][i2]*m[j2][i1]); } } } #define rfabs(x) ((x < 0) ? -x : x) #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); /////////////////////////////////////////////////////////////////////////// /// Meigen /// /////////////////////////////////////////////////////////////////////////// /// /// Determine les vecteurs à partir de la matrice de covariance /// int inline Meigen(float vout[3][3], float dout[3], float a[3][3]) { int i; float tresh, theta, tau, t, sm, s, h, g, c; int nrot; float b[3]; float z[3]; float v[3][3]; float d[3]; v[0][0] = v[1][1] = v[2][2] = 1.0f; v[0][1] = v[1][2] = v[2][0] = 0.0f; v[0][2] = v[1][0] = v[2][1] = 0.0f; b[0] = a[0][0]; d[0] = a[0][0]; z[0] = 0.0f; b[1] = a[1][1]; d[1] = a[1][1]; z[1] = 0.0f; b[2] = a[2][2]; d[2] = a[2][2]; z[2] = 0.0f; nrot = 0; for(i=0; i<50; i++) { sm = 0.0; sm += (float) fabs(a[0][1]); sm += (float) fabs(a[0][2]); sm += (float) fabs(a[1][2]); if(sm == 0.0) { McM(vout,v); VcV(dout,d); return i; } if(i<3) tresh = 0.2f*sm/(3*3); else tresh = 0.0f; { g = 100.0f * rfabs(a[0][1]); if(i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[1])+g==rfabs(d[1])) a[0][1] = 0.0f; else if(rfabs(a[0][1])>tresh) { h = d[1]-d[0]; if(rfabs(h)+g == rfabs(h)) t = (a[0][1])/h; else { theta = 0.5f*h/(a[0][1]); t = 1.0f/(float)(rfabs(theta)+sqrt(1.0+theta*theta)); if(theta < 0.0f) t = -t; } c = 1.0f/(float)sqrt(1+t*t); s = t*c; tau = s/(1.0f+c); h = t*a[0][1]; z[0] -= h; z[1] += h; d[0] -= h; d[1] += h; a[0][1]=0.0; ROT(a,0,2,1,2); ROT(v,0,0,0,1); ROT(v,1,0,1,1); ROT(v,2,0,2,1); nrot++; } } { g = 100.0f * rfabs(a[0][2]); if(i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[2])+g==rfabs(d[2])) a[0][2]=0.0f; else if (rfabs(a[0][2])>tresh) { h = d[2]-d[0]; if(rfabs(h)+g == rfabs(h)) t = (a[0][2])/h; else { theta = 0.5f*h/(a[0][2]); t = 1.0f/(float)(rfabs(theta)+sqrt(1.0+theta*theta)); if(theta < 0.0f) t = -t; } c = 1.0f/(float)sqrt(1+t*t); s = t*c; tau = s/(1.0f+c); h = t*a[0][2]; z[0] -= h; z[2] += h; d[0] -= h; d[2] += h; a[0][2]=0.0; ROT(a,0,1,1,2); ROT(v,0,0,0,2); ROT(v,1,0,1,2); ROT(v,2,0,2,2); nrot++; } } { g = 100.0f * rfabs(a[1][2]); if(i>3 && rfabs(d[1])+g==rfabs(d[1]) && rfabs(d[2])+g==rfabs(d[2])) a[1][2]=0.0f; else if (rfabs(a[1][2])>tresh) { h = d[2]-d[1]; if(rfabs(h)+g == rfabs(h)) t = (a[1][2])/h; else { theta = 0.5f*h/(a[1][2]); t = 1.0f/(float)(rfabs(theta)+sqrt(1.0+theta*theta)); if(theta < 0.0f) t = -t; } c = 1.0f/(float)sqrt(1+t*t); s = t*c; tau=s/(1.0f+c); h = t*a[1][2]; z[1] -= h; z[2] += h; d[1] -= h; d[2] += h; a[1][2]=0.0f; ROT(a,0,1,0,2); ROT(v,0,1,0,2); ROT(v,1,1,1,2); ROT(v,2,1,2,2); nrot++; } } b[0] += z[0]; d[0] = b[0]; z[0] = 0.0f; b[1] += z[1]; d[1] = b[1]; z[1] = 0.0f; b[2] += z[2]; d[2] = b[2]; z[2] = 0.0f; } fprintf(stderr, "eigen: too many iterations in Jacobi transform (%d).\n", i); return i; } #endif