source: trunk/sys/TinyGL/src/zmath.c @ 150

Last change on this file since 150 was 1, checked in by alain, 8 years ago

First import

File size: 5.3 KB
Line 
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
12void 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
20int 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
32void 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 */
45void 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
63void gl_M4_Move(M4 *a,M4 *b)
64{
65        memcpy(a,b,sizeof(M4));
66}
67
68void gl_MoveV3(V3 *a,V3 *b)
69{
70        memcpy(a,b,sizeof(V3));
71}
72
73
74void 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
81void 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
88void 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 */
97void 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 */ 
121void 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
138int 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
198void 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
206void 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 */
221void 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
245int 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
256V3 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
265V4 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
Note: See TracBrowser for help on using the repository browser.