00001
00002
00003
00004 #include <stdlib.h>
00005 #include <string.h>
00006 #include <math.h>
00007 #include "zmath.h"
00008
00009
00010
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
00045 void gl_M4_MulLeft(M4 *c,M4 *b)
00046 {
00047 int i,j,k;
00048 PN_stdfloat s;
00049 M4 a;
00050
00051
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
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
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
00136
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
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
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
00159 if (max==0) return 1;
00160
00161
00162
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
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
00197
00198 void gl_M4_Inv(M4 *a,M4 *b)
00199 {
00200 M4 tmp;
00201 memcpy(&tmp, b, 16*sizeof(PN_stdfloat));
00202
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
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
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