Subversion Repositories Kolibri OS

Rev

Blame | Last modification | View Log | Download | RSS feed

  1. /* Some simple mathematical functions. Don't look for some logic in
  2.    the function names :-) */
  3.  
  4. #include <stdlib.h>
  5. #include <string.h>
  6. #include <math.h>
  7. #include "zmath.h"
  8.  
  9.  
  10. /* ******* Gestion des matrices 4x4 ****** */
  11.  
  12. void gl_M4_Id(M4 *a)
  13. {
  14.         int i,j;
  15.         for(i=0;i<4;i++)
  16.         for(j=0;j<4;j++)
  17.         if (i==j) a->m[i][j]=1.0; else a->m[i][j]=0.0;
  18. }
  19.  
  20. int gl_M4_IsId(M4 *a)
  21. {
  22.         int i,j;
  23.         for(i=0;i<4;i++)
  24.     for(j=0;j<4;j++) {
  25.       if (i==j) {
  26.         if (a->m[i][j] != 1.0) return 0;
  27.       } else if (a->m[i][j] != 0.0) return 0;
  28.     }
  29.   return 1;
  30. }
  31.  
  32. void gl_M4_Mul(M4 *c,M4 *a,M4 *b)
  33. {
  34.   int i,j,k;
  35.   float s;
  36.   for(i=0;i<4;i++)
  37.     for(j=0;j<4;j++) {
  38.       s=0.0;
  39.       for(k=0;k<4;k++) s+=a->m[i][k]*b->m[k][j];
  40.       c->m[i][j]=s;
  41.     }
  42. }
  43.  
  44. /* c=c*a */
  45. void gl_M4_MulLeft(M4 *c,M4 *b)
  46. {
  47.   int i,j,k;
  48.   float s;
  49.   M4 a;
  50.  
  51.   /*memcpy(&a, c, 16*sizeof(float));
  52.   */
  53.   a=*c;
  54.  
  55.   for(i=0;i<4;i++)
  56.     for(j=0;j<4;j++) {
  57.       s=0.0;
  58.       for(k=0;k<4;k++) s+=a.m[i][k]*b->m[k][j];
  59.       c->m[i][j]=s;
  60.     }
  61. }
  62.  
  63. void gl_M4_Move(M4 *a,M4 *b)
  64. {
  65.         memcpy(a,b,sizeof(M4));
  66. }
  67.  
  68. void gl_MoveV3(V3 *a,V3 *b)
  69. {
  70.         memcpy(a,b,sizeof(V3));
  71. }
  72.  
  73.  
  74. void gl_MulM4V3(V3 *a,M4 *b,V3 *c)
  75. {
  76.          a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3];
  77.          a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3];
  78.          a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3];
  79. }
  80.  
  81. void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
  82. {
  83.          a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z;
  84.          a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z;
  85.          a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z;
  86. }
  87.  
  88. void gl_M4_MulV4(V4 *a,M4 *b,V4 *c)
  89. {
  90.          a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3]*c->W;
  91.          a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3]*c->W;
  92.          a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3]*c->W;
  93.          a->W=b->m[3][0]*c->X+b->m[3][1]*c->Y+b->m[3][2]*c->Z+b->m[3][3]*c->W;
  94. }
  95.        
  96. /* transposition of a 4x4 matrix */
  97. void gl_M4_Transpose(M4 *a,M4 *b)
  98. {
  99.   a->m[0][0]=b->m[0][0];
  100.   a->m[0][1]=b->m[1][0];
  101.   a->m[0][2]=b->m[2][0];
  102.   a->m[0][3]=b->m[3][0];
  103.  
  104.   a->m[1][0]=b->m[0][1];
  105.   a->m[1][1]=b->m[1][1];
  106.   a->m[1][2]=b->m[2][1];
  107.   a->m[1][3]=b->m[3][1];
  108.  
  109.   a->m[2][0]=b->m[0][2];
  110.   a->m[2][1]=b->m[1][2];
  111.   a->m[2][2]=b->m[2][2];
  112.   a->m[2][3]=b->m[3][2];
  113.  
  114.   a->m[3][0]=b->m[0][3];
  115.   a->m[3][1]=b->m[1][3];
  116.   a->m[3][2]=b->m[2][3];
  117.   a->m[3][3]=b->m[3][3];
  118. }
  119.  
  120. /* inversion of an orthogonal matrix of type Y=M.X+P */
  121. void gl_M4_InvOrtho(M4 *a,M4 b)
  122. {
  123.         int i,j;
  124.         float s;
  125.         for(i=0;i<3;i++)
  126.         for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
  127.         a->m[3][0]=0.0; a->m[3][1]=0.0; a->m[3][2]=0.0; a->m[3][3]=1.0;
  128.         for(i=0;i<3;i++) {
  129.                 s=0;
  130.                 for(j=0;j<3;j++) s-=b.m[j][i]*b.m[j][3];
  131.                 a->m[i][3]=s;
  132.         }
  133. }
  134.  
  135. /* Inversion of a general nxn matrix.
  136.    Note : m is destroyed */
  137.  
  138. int Matrix_Inv(float *r,float *m,int n)
  139. {
  140.          int i,j,k,l;
  141.          float max,tmp,t;
  142.  
  143.          /* identitée dans r */
  144.          for(i=0;i<n*n;i++) r[i]=0;
  145.          for(i=0;i<n;i++) r[i*n+i]=1;
  146.          
  147.          for(j=0;j<n;j++) {
  148.                        
  149.                         /* recherche du nombre de plus grand module sur la colonne j */
  150.                         max=m[j*n+j];
  151.                         k=j;
  152.                         for(i=j+1;i<n;i++)
  153.                                 if (fabs(m[i*n+j])>fabs(max)) {
  154.                                          k=i;
  155.                                          max=m[i*n+j];
  156.                                 }
  157.  
  158.       /* non intersible matrix */
  159.       if (max==0) return 1;
  160.  
  161.                        
  162.                         /* permutation des lignes j et k */
  163.                         if (k!=j) {
  164.                                  for(i=0;i<n;i++) {
  165.                                                 tmp=m[j*n+i];
  166.                                                 m[j*n+i]=m[k*n+i];
  167.                                                 m[k*n+i]=tmp;
  168.                                                
  169.                                                 tmp=r[j*n+i];
  170.                                                 r[j*n+i]=r[k*n+i];
  171.                                                 r[k*n+i]=tmp;
  172.                                  }
  173.                         }
  174.                        
  175.                         /* multiplication de la ligne j par 1/max */
  176.                         max=1/max;
  177.                         for(i=0;i<n;i++) {
  178.                                  m[j*n+i]*=max;
  179.                                  r[j*n+i]*=max;
  180.                         }
  181.                        
  182.                        
  183.                         for(l=0;l<n;l++) if (l!=j) {
  184.                                  t=m[l*n+j];
  185.                                  for(i=0;i<n;i++) {
  186.                                                 m[l*n+i]-=m[j*n+i]*t;
  187.                                                 r[l*n+i]-=r[j*n+i]*t;
  188.                                  }
  189.                         }
  190.          }
  191.  
  192.          return 0;
  193. }
  194.  
  195.  
  196. /* inversion of a 4x4 matrix */
  197.  
  198. void gl_M4_Inv(M4 *a,M4 *b)
  199. {
  200.   M4 tmp;
  201.   memcpy(&tmp, b, 16*sizeof(float));
  202.   Matrix_Inv(&a->m[0][0],&tmp.m[0][0],4);
  203. }
  204.  
  205. void gl_M4_Rotate(M4 *a,float t,int u)
  206. {
  207.          float s,c;
  208.          int v,w;
  209.    if ((v=u+1)>2) v=0;
  210.          if ((w=v+1)>2) w=0;
  211.          s=sin(t);
  212.          c=cos(t);
  213.          gl_M4_Id(a);
  214.          a->m[v][v]=c;  a->m[v][w]=-s;
  215.          a->m[w][v]=s;  a->m[w][w]=c;
  216. }
  217.        
  218.  
  219. /* inverse of a 3x3 matrix */
  220. void gl_M3_Inv(M3 *a,M3 *m)
  221. {
  222.          float det;
  223.          
  224.          det = m->m[0][0]*m->m[1][1]*m->m[2][2]-m->m[0][0]*m->m[1][2]*m->m[2][1]-
  225.                  m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
  226.                  m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];
  227.  
  228.          a->m[0][0] = (m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
  229.          a->m[0][1] = -(m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
  230.          a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
  231.          
  232.          a->m[1][0] = -(m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
  233.          a->m[1][1] = (m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
  234.          a->m[1][2] = -(m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;
  235.  
  236.          a->m[2][0] = (m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
  237.          a->m[2][1] = -(m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
  238.          a->m[2][2] = (m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
  239. }
  240.  
  241.                                                                                                                                                                                                                
  242. /* vector arithmetic */
  243.  
  244. int gl_V3_Norm(V3 *a)
  245. {
  246.         float n;
  247.         n=sqrt(a->X*a->X+a->Y*a->Y+a->Z*a->Z);
  248.         if (n==0) return 1;
  249.         a->X/=n;
  250.         a->Y/=n;
  251.         a->Z/=n;
  252.         return 0;
  253. }
  254.  
  255. V3 gl_V3_New(float x,float y,float z)
  256. {
  257.          V3 a;
  258.          a.X=x;
  259.          a.Y=y;
  260.          a.Z=z;
  261.          return a;
  262. }
  263.  
  264. V4 gl_V4_New(float x,float y,float z,float w)
  265. {
  266.   V4 a;
  267.   a.X=x;
  268.   a.Y=y;
  269.   a.Z=z;
  270.   a.W=w;
  271.   return a;
  272. }
  273.  
  274.  
  275.