Panda3D
|
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