version 2.8
zmath.c
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  /*tmp=*b;*/
203  Matrix_Inv(&a->m[0][0],&tmp.m[0][0],4);
204 }
205 
206 void gl_M4_Rotate(M4 *a,float t,int u)
207 {
208  float s,c;
209  int v,w;
210  if ((v=u+1)>2) v=0;
211  if ((w=v+1)>2) w=0;
212  s=sin(t);
213  c=cos(t);
214  gl_M4_Id(a);
215  a->m[v][v]=c; a->m[v][w]=-s;
216  a->m[w][v]=s; a->m[w][w]=c;
217 }
218 
219 
220 /* inverse of a 3x3 matrix */
221 void gl_M3_Inv(M3 *a,M3 *m)
222 {
223  float det;
224 
225  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]-
226  m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
227  m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];
228 
229  a->m[0][0] = (m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
230  a->m[0][1] = -(m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
231  a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
232 
233  a->m[1][0] = -(m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
234  a->m[1][1] = (m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
235  a->m[1][2] = -(m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;
236 
237  a->m[2][0] = (m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
238  a->m[2][1] = -(m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
239  a->m[2][2] = (m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
240 }
241 
242 
243 /* vector arithmetic */
244 
245 int gl_V3_Norm(V3 *a)
246 {
247  float n;
248  n=sqrt(a->X*a->X+a->Y*a->Y+a->Z*a->Z);
249  if (n==0) return 1;
250  a->X/=n;
251  a->Y/=n;
252  a->Z/=n;
253  return 0;
254 }
255 
256 V3 gl_V3_New(float x,float y,float z)
257 {
258  V3 a;
259  a.X=x;
260  a.Y=y;
261  a.Z=z;
262  return a;
263 }
264 
265 V4 gl_V4_New(float x,float y,float z,float w)
266 {
267  V4 a;
268  a.X=x;
269  a.Y=y;
270  a.Z=z;
271  a.W=w;
272  return a;
273 }
274 
275