Panda3D

zmath.cxx

00001 /* Some simple mathematical functions. Don't look for some logic in
00002    the function names :-) */
00003 
00004 #include <stdlib.h>
00005 #include <string.h>
00006 #include <math.h>
00007 #include "zmath.h"
00008 
00009 
00010 /* ******* Gestion des matrices 4x4 ****** */
00011 
00012 void gl_M4_Id(M4 *a)
00013 {
00014   int i,j;
00015   for(i=0;i<4;i++)
00016     for(j=0;j<4;j++) 
00017       if (i==j) a->m[i][j]=1.0; else a->m[i][j]=0.0;
00018 }
00019 
00020 int gl_M4_IsId(M4 *a)
00021 {
00022   int i,j;
00023   for(i=0;i<4;i++)
00024     for(j=0;j<4;j++) {
00025       if (i==j) { 
00026         if (a->m[i][j] != 1.0) return 0;
00027       } else if (a->m[i][j] != 0.0) return 0;
00028     }
00029   return 1;
00030 }
00031 
00032 void gl_M4_Mul(M4 *c,M4 *a,M4 *b)
00033 {
00034   int i,j,k;
00035   PN_stdfloat s;
00036   for(i=0;i<4;i++)
00037     for(j=0;j<4;j++) {
00038       s=0.0;
00039       for(k=0;k<4;k++) s+=a->m[i][k]*b->m[k][j];
00040       c->m[i][j]=s;
00041     }
00042 }
00043 
00044 /* c=c*a */
00045 void gl_M4_MulLeft(M4 *c,M4 *b)
00046 {
00047   int i,j,k;
00048   PN_stdfloat s;
00049   M4 a;
00050 
00051   /*memcpy(&a, c, 16*sizeof(PN_stdfloat));
00052    */
00053   a=*c;
00054 
00055   for(i=0;i<4;i++)
00056     for(j=0;j<4;j++) {
00057       s=0.0;
00058       for(k=0;k<4;k++) s+=a.m[i][k]*b->m[k][j];
00059       c->m[i][j]=s;
00060     }
00061 }
00062 
00063 void gl_M4_Move(M4 *a,M4 *b)
00064 {
00065   memcpy(a,b,sizeof(M4));
00066 }
00067 
00068 void gl_MoveV3(V3 *a,V3 *b)
00069 {
00070   memcpy(a,b,sizeof(V3));
00071 }
00072 
00073 
00074 void gl_MulM4V3(V3 *a,M4 *b,V3 *c)
00075 {
00076   a->v[0]=b->m[0][0]*c->v[0]+b->m[0][1]*c->v[1]+b->m[0][2]*c->v[2]+b->m[0][3];
00077   a->v[1]=b->m[1][0]*c->v[0]+b->m[1][1]*c->v[1]+b->m[1][2]*c->v[2]+b->m[1][3];
00078   a->v[2]=b->m[2][0]*c->v[0]+b->m[2][1]*c->v[1]+b->m[2][2]*c->v[2]+b->m[2][3];
00079 }
00080 
00081 void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
00082 {
00083   a->v[0]=b->m[0][0]*c->v[0]+b->m[0][1]*c->v[1]+b->m[0][2]*c->v[2];
00084   a->v[1]=b->m[1][0]*c->v[0]+b->m[1][1]*c->v[1]+b->m[1][2]*c->v[2];
00085   a->v[2]=b->m[2][0]*c->v[0]+b->m[2][1]*c->v[1]+b->m[2][2]*c->v[2];
00086 }
00087 
00088 void gl_M4_MulV4(V4 *a,M4 *b,V4 *c)
00089 {
00090   a->v[0]=b->m[0][0]*c->v[0]+b->m[0][1]*c->v[1]+b->m[0][2]*c->v[2]+b->m[0][3]*c->v[3];
00091   a->v[1]=b->m[1][0]*c->v[0]+b->m[1][1]*c->v[1]+b->m[1][2]*c->v[2]+b->m[1][3]*c->v[3];
00092   a->v[2]=b->m[2][0]*c->v[0]+b->m[2][1]*c->v[1]+b->m[2][2]*c->v[2]+b->m[2][3]*c->v[3];
00093   a->v[3]=b->m[3][0]*c->v[0]+b->m[3][1]*c->v[1]+b->m[3][2]*c->v[2]+b->m[3][3]*c->v[3];
00094 }
00095 
00096 /* transposition of a 4x4 matrix */
00097 void gl_M4_Transpose(M4 *a,M4 *b)
00098 {
00099   a->m[0][0]=b->m[0][0]; 
00100   a->m[0][1]=b->m[1][0]; 
00101   a->m[0][2]=b->m[2][0]; 
00102   a->m[0][3]=b->m[3][0]; 
00103 
00104   a->m[1][0]=b->m[0][1]; 
00105   a->m[1][1]=b->m[1][1]; 
00106   a->m[1][2]=b->m[2][1]; 
00107   a->m[1][3]=b->m[3][1]; 
00108 
00109   a->m[2][0]=b->m[0][2]; 
00110   a->m[2][1]=b->m[1][2]; 
00111   a->m[2][2]=b->m[2][2]; 
00112   a->m[2][3]=b->m[3][2]; 
00113 
00114   a->m[3][0]=b->m[0][3]; 
00115   a->m[3][1]=b->m[1][3]; 
00116   a->m[3][2]=b->m[2][3]; 
00117   a->m[3][3]=b->m[3][3]; 
00118 }
00119 
00120 /* inversion of an orthogonal matrix of type Y=M.X+P */ 
00121 void gl_M4_InvOrtho(M4 *a,M4 b)
00122 {
00123   int i,j;
00124   PN_stdfloat s;
00125   for(i=0;i<3;i++)
00126     for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
00127   a->m[3][0]=0.0; a->m[3][1]=0.0; a->m[3][2]=0.0; a->m[3][3]=1.0;
00128   for(i=0;i<3;i++) {
00129     s=0;
00130     for(j=0;j<3;j++) s-=b.m[j][i]*b.m[j][3];
00131     a->m[i][3]=s;
00132   }
00133 }
00134 
00135 /* Inversion of a general nxn matrix.
00136    Note : m is destroyed */
00137 
00138 int Matrix_Inv(PN_stdfloat *r,PN_stdfloat *m,int n)
00139 {
00140   int i,j,k,l;
00141   PN_stdfloat max,tmp,t;
00142 
00143   /* identitée dans r */
00144   for(i=0;i<n*n;i++) r[i]=0;
00145   for(i=0;i<n;i++) r[i*n+i]=1;
00146 
00147   for(j=0;j<n;j++) {
00148 
00149     /* recherche du nombre de plus grand module sur la colonne j */
00150     max=m[j*n+j];
00151     k=j;
00152     for(i=j+1;i<n;i++)
00153       if (fabs(m[i*n+j])>fabs(max)) {
00154         k=i;
00155         max=m[i*n+j];
00156       }
00157 
00158     /* non intersible matrix */
00159     if (max==0) return 1;
00160 
00161 
00162     /* permutation des lignes j et k */
00163     if (k!=j) {
00164       for(i=0;i<n;i++) {
00165         tmp=m[j*n+i];
00166         m[j*n+i]=m[k*n+i];
00167         m[k*n+i]=tmp;
00168 
00169         tmp=r[j*n+i];
00170         r[j*n+i]=r[k*n+i];
00171         r[k*n+i]=tmp;
00172       }
00173     }
00174 
00175     /* multiplication de la ligne j par 1/max */
00176     max=1/max;
00177     for(i=0;i<n;i++) {
00178       m[j*n+i]*=max;
00179       r[j*n+i]*=max;
00180     }
00181 
00182 
00183     for(l=0;l<n;l++) if (l!=j) {
00184         t=m[l*n+j];
00185         for(i=0;i<n;i++) {
00186           m[l*n+i]-=m[j*n+i]*t;
00187           r[l*n+i]-=r[j*n+i]*t;
00188         }
00189       }
00190   }
00191 
00192   return 0;
00193 }
00194 
00195 
00196 /* inversion of a 4x4 matrix */
00197 
00198 void gl_M4_Inv(M4 *a,M4 *b)
00199 {
00200   M4 tmp;
00201   memcpy(&tmp, b, 16*sizeof(PN_stdfloat));
00202   /*tmp=*b;*/
00203   Matrix_Inv(&a->m[0][0],&tmp.m[0][0],4);
00204 }
00205 
00206 void gl_M4_Rotate(M4 *a,PN_stdfloat t,int u)
00207 {
00208   PN_stdfloat s,c;
00209   int v,w;
00210   if ((v=u+1)>2) v=0;
00211   if ((w=v+1)>2) w=0;
00212   s=sin(t);
00213   c=cos(t);
00214   gl_M4_Id(a);
00215   a->m[v][v]=c;  a->m[v][w]=-s;
00216   a->m[w][v]=s;  a->m[w][w]=c;
00217 }
00218 
00219 
00220 /* inverse of a 3x3 matrix */
00221 void gl_M3_Inv(M3 *a,M3 *m)
00222 {
00223   PN_stdfloat det;
00224 
00225   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]-
00226     m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
00227     m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];
00228 
00229   a->m[0][0] = (m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
00230   a->m[0][1] = -(m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
00231   a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
00232 
00233   a->m[1][0] = -(m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
00234   a->m[1][1] = (m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
00235   a->m[1][2] = -(m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;
00236 
00237   a->m[2][0] = (m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
00238   a->m[2][1] = -(m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
00239   a->m[2][2] = (m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
00240 }
00241 
00242 
00243 /* vector arithmetic */
00244 
00245 int gl_V3_Norm(V3 *a)
00246 {
00247   PN_stdfloat n;
00248   n=sqrt(a->v[0]*a->v[0]+a->v[1]*a->v[1]+a->v[2]*a->v[2]);
00249   if (n==0) return 1;
00250   a->v[0]/=n;
00251   a->v[1]/=n;
00252   a->v[2]/=n;
00253   return 0;
00254 }
00255 
00256 V3 gl_V3_New(PN_stdfloat x,PN_stdfloat y,PN_stdfloat z)
00257 {
00258   V3 a;
00259   a.v[0]=x;
00260   a.v[1]=y;
00261   a.v[2]=z;
00262   return a;
00263 }
00264 
00265 V4 gl_V4_New(PN_stdfloat x,PN_stdfloat y,PN_stdfloat z,PN_stdfloat w)
00266 {
00267   V4 a;
00268   a.v[0]=x;
00269   a.v[1]=y;
00270   a.v[2]=z;
00271   a.v[3]=w;
00272   return a;
00273 }
00274 
00275 
 All Classes Functions Variables Enumerations