~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/math/geom/mat4.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)Mat4 handling
  2 **/
  3 
  4 #include <stdio.h>
  5 #include <string.h>
  6 #include <math.h>
  7 #include <tina/sys.h>
  8 #include <tina/sysfuncs.h>
  9 #include <tina/math.h>
 10 #include <tina/mathfuncs.h>
 11 
 12 static Mat4 mat4_0 = {Mat4_id};
 13 
 14 Mat4   *mat4_alloc(void)
 15 {
 16     Mat4   *m = ts_ralloc(Mat4);
 17 
 18     m->el[0][0] = 0.0;
 19     m->el[0][1] = 0.0;
 20     m->el[0][2] = 0.0;
 21     m->el[0][3] = 0.0;
 22     m->el[1][0] = 0.0;
 23     m->el[1][1] = 0.0;
 24     m->el[1][2] = 0.0;
 25     m->el[1][3] = 0.0;
 26     m->el[2][0] = 0.0;
 27     m->el[2][1] = 0.0;
 28     m->el[2][2] = 0.0;
 29     m->el[2][3] = 0.0;
 30     m->el[3][0] = 0.0;
 31     m->el[3][1] = 0.0;
 32     m->el[3][2] = 0.0;
 33     m->el[3][3] = 0.0;
 34     return (m);
 35 }
 36 
 37 Mat4   *mat4_make(Mat4 n)
 38 {
 39     Mat4   *m = ts_ralloc(Mat4);
 40 
 41     *m = n;
 42     return (m);
 43 }
 44 
 45 void    mat4_free(Mat4 * m)
 46 {
 47     rfree((void *) m);
 48 }
 49 
 50 /** function versions of component macros **/
 51 
 52 double  mat4_get_xx(Mat4 m)
 53 {
 54     return (mat4_xx(m));
 55 }
 56 
 57 double  mat4_get_xy(Mat4 m)
 58 {
 59     return (mat4_xy(m));
 60 }
 61 
 62 double  mat4_get_xz(Mat4 m)
 63 {
 64     return (mat4_xz(m));
 65 }
 66 
 67 double  mat4_get_xw(Mat4 m)
 68 {
 69     return (mat4_xw(m));
 70 }
 71 
 72 double  mat4_get_yx(Mat4 m)
 73 {
 74     return (mat4_yx(m));
 75 }
 76 
 77 double  mat4_get_yy(Mat4 m)
 78 {
 79     return (mat4_yy(m));
 80 }
 81 
 82 double  mat4_get_yz(Mat4 m)
 83 {
 84     return (mat4_yz(m));
 85 }
 86 
 87 double  mat4_get_yw(Mat4 m)
 88 {
 89     return (mat4_yw(m));
 90 }
 91 
 92 double  mat4_get_zx(Mat4 m)
 93 {
 94     return (mat4_zx(m));
 95 }
 96 
 97 double  mat4_get_zy(Mat4 m)
 98 {
 99     return (mat4_zy(m));
100 }
101 
102 double  mat4_get_zz(Mat4 m)
103 {
104     return (mat4_zz(m));
105 }
106 
107 double  mat4_get_zw(Mat4 m)
108 {
109     return (mat4_zw(m));
110 }
111 
112 double  mat4_get_wx(Mat4 m)
113 {
114     return (mat4_wx(m));
115 }
116 
117 double  mat4_get_wy(Mat4 m)
118 {
119     return (mat4_wy(m));
120 }
121 
122 double  mat4_get_wz(Mat4 m)
123 {
124     return (mat4_wz(m));
125 }
126 
127 double  mat4_get_ww(Mat4 m)
128 {
129     return (mat4_ww(m));
130 }
131 
132 Mat4    mat4(double mxx, double mxy, double mxz, double mxw, double myx, double myy, double myz, double myw, double mzx, double mzy, double mzz, double mzw, double mwx, double mwy, double mwz, double mww)
133 {
134     Mat4    m = {Mat4_id};
135 
136     m.el[0][0] = mxx;
137     m.el[0][1] = mxy;
138     m.el[0][2] = mxz;
139     m.el[0][3] = mxw;
140     m.el[1][0] = myx;
141     m.el[1][1] = myy;
142     m.el[1][2] = myz;
143     m.el[1][3] = myw;
144     m.el[2][0] = mzx;
145     m.el[2][1] = mzy;
146     m.el[2][2] = mzz;
147     m.el[2][3] = mzw;
148     m.el[3][0] = mwx;
149     m.el[3][1] = mwy;
150     m.el[3][2] = mwz;
151     m.el[3][3] = mww;
152     return (m);
153 }
154 
155 Mat4    mat4_unit(void)
156 {
157     return (mat4(1.0, 0.0, 0.0, 0.0,
158                  0.0, 1.0, 0.0, 0.0,
159                  0.0, 0.0, 1.0, 0.0,
160                  0.0, 0.0, 0.0, 1.0));
161 }
162 
163 Mat4    mat4_zero(void)
164 {
165     return (mat4_0);
166 }
167 
168 void    mat4_comps(Mat4 m, double *mxx, double *mxy, double *mxz, double *mxw, double *myx, double *myy, double *myz, double *myw, double *mzx, double *mzy, double *mzz, double *mzw, double *mwx, double *mwy, double *mwz, double *mww)
169 {
170     *mxx = m.el[0][0];
171     *mxy = m.el[0][1];
172     *mxz = m.el[0][2];
173     *mxw = m.el[0][3];
174     *myx = m.el[1][0];
175     *myy = m.el[1][1];
176     *myz = m.el[1][2];
177     *myw = m.el[1][3];
178     *mzx = m.el[2][0];
179     *mzy = m.el[2][1];
180     *mzz = m.el[2][2];
181     *mzw = m.el[2][3];
182     *mwx = m.el[3][0];
183     *mwy = m.el[3][1];
184     *mwz = m.el[3][2];
185     *mww = m.el[3][3];
186 }
187 
188 Vec4    mat4_rowx(Mat4 m)
189 {
190     return (vec4(m.el[0][0], m.el[0][1], m.el[0][2], m.el[0][3]));
191 }
192 
193 Vec4    mat4_rowy(Mat4 m)
194 {
195     return (vec4(m.el[1][0], m.el[1][1], m.el[1][2], m.el[1][3]));
196 }
197 
198 Vec4    mat4_rowz(Mat4 m)
199 {
200     return (vec4(m.el[2][0], m.el[2][1], m.el[2][2], m.el[2][3]));
201 }
202 
203 Vec4    mat4_roww(Mat4 m)
204 {
205     return (vec4(m.el[3][0], m.el[3][1], m.el[3][2], m.el[3][3]));
206 }
207 
208 Vec4    mat4_colx(Mat4 m)
209 {
210     return (vec4(m.el[0][0], m.el[1][0], m.el[2][0], m.el[3][0]));
211 }
212 
213 Vec4    mat4_coly(Mat4 m)
214 {
215     return (vec4(m.el[0][1], m.el[1][1], m.el[2][1], m.el[3][1]));
216 }
217 
218 Vec4    mat4_colz(Mat4 m)
219 {
220     return (vec4(m.el[0][2], m.el[1][2], m.el[2][2], m.el[3][2]));
221 }
222 
223 Vec4    mat4_colw(Mat4 m)
224 {
225     return (vec4(m.el[0][3], m.el[1][3], m.el[2][3], m.el[3][3]));
226 }
227 
228 Mat4    mat4_of_cols(Vec4 cx, Vec4 cy, Vec4 cz, Vec4 cw)
229 {
230     Mat4    m = {Mat4_id};
231 
232     m.el[0][0] = cx.el[0];
233     m.el[1][0] = cx.el[1];
234     m.el[2][0] = cx.el[2];
235     m.el[3][0] = cx.el[3];
236     m.el[0][1] = cy.el[0];
237     m.el[1][1] = cy.el[1];
238     m.el[2][1] = cy.el[2];
239     m.el[3][1] = cy.el[3];
240     m.el[0][2] = cz.el[0];
241     m.el[1][2] = cz.el[1];
242     m.el[2][2] = cz.el[2];
243     m.el[3][2] = cz.el[3];
244     m.el[0][3] = cw.el[0];
245     m.el[1][3] = cw.el[1];
246     m.el[2][3] = cw.el[2];
247     m.el[3][3] = cw.el[3];
248     return (m);
249 }
250 
251 Mat4    mat4_of_rows(Vec4 rx, Vec4 ry, Vec4 rz, Vec4 rw)
252 {
253     Mat4    m = {Mat4_id};
254 
255     m.el[0][0] = rx.el[0];
256     m.el[0][1] = rx.el[1];
257     m.el[0][2] = rx.el[2];
258     m.el[0][3] = rx.el[3];
259     m.el[1][0] = ry.el[0];
260     m.el[1][1] = ry.el[1];
261     m.el[1][2] = ry.el[2];
262     m.el[1][3] = ry.el[3];
263     m.el[2][0] = rz.el[0];
264     m.el[2][1] = rz.el[1];
265     m.el[2][2] = rz.el[2];
266     m.el[2][3] = rz.el[3];
267     m.el[3][0] = rw.el[0];
268     m.el[3][1] = rw.el[1];
269     m.el[3][2] = rw.el[2];
270     m.el[3][3] = rw.el[3];
271     return (m);
272 }
273 
274 Mat4    mat4_sum(Mat4 m, Mat4 n)
275 {
276     Mat4    sum = {Mat4_id};
277     int     i, j;
278 
279     for (i = 0; i < 4; ++i)
280         for (j = 0; j < 4; ++j)
281             sum.el[i][j] = m.el[i][j] + n.el[i][j];
282     return (sum);
283 }
284 
285 Mat4    mat4_diff(Mat4 m, Mat4 n)
286 {
287     Mat4    diff = {Mat4_id};
288     int     i, j;
289 
290     for (i = 0; i < 4; ++i)
291         for (j = 0; j < 4; ++j)
292             diff.el[i][j] = m.el[i][j] - n.el[i][j];
293     return (diff);
294 }
295 
296 Mat4    mat4_minus(Mat4 m)
297 {
298     Mat4    minus = {Mat4_id};
299     int     i, j;
300 
301     for (i = 0; i < 4; ++i)
302         for (j = 0; j < 4; ++j)
303             minus.el[i][j] = -m.el[i][j];
304     return (minus);
305 }
306 
307 Mat4    mat4_times(double k, Mat4 m)
308 {
309     Mat4    prod = {Mat4_id};
310     int     i, j;
311 
312     for (i = 0; i < 4; ++i)
313         for (j = 0; j < 4; ++j)
314             prod.el[i][j] = k * m.el[i][j];
315     return (prod);
316 }
317 
318 Mat4    mat4_prod(Mat4 m, Mat4 n)
319 {
320     Mat4    prod = {Mat4_id};
321     double  sum;
322     int     i, j, k;
323 
324     for (i = 0; i < 4; ++i)
325         for (j = 0; j < 4; ++j)
326         {
327             sum = 0.0;
328             for (k = 0; k < 4; ++k)
329                 sum += m.el[i][k] * n.el[k][j];
330             prod.el[i][j] = sum;
331         }
332     return (prod);
333 }
334 
335 Mat4    mat4_inverse(Mat4 m)
336 {
337     Mat4    inv = {Mat4_id};
338     double  a[4][4];
339     int     i, j, k, p[4];
340     double  h, q, s, sup, pivot;
341 
342     for (i = 0; i < 4; i++)
343         for (j = 0; j < 4; j++)
344             a[i][j] = m.el[i][j];
345 
346     for (k = 0; k < 4; k++)
347     {
348         sup = 0.0;
349         p[k] = 0;
350         for (i = k; i < 4; i++)
351         {
352             s = 0.0;
353             for (j = k; j < 4; j++)
354                 s += fabs(a[i][j]);
355             q = fabs(a[i][k]) / s;
356             if (sup < q)
357             {
358                 sup = q;
359                 p[k] = i;
360             }
361         }
362         if (sup == 0.0)
363             return (mat4_unit());
364         if (p[k] != k)
365             for (j = 0; j < 4; j++)
366             {
367                 h = a[k][j];
368                 a[k][j] = a[p[k]][j];
369                 a[p[k]][j] = h;
370             }
371         pivot = a[k][k];
372         for (j = 0; j < 4; j++)
373             if (j != k)
374             {
375                 a[k][j] = -a[k][j] / pivot;
376                 for (i = 0; i < 4; i++)
377                     if (i != k)
378                         a[i][j] += a[i][k] * a[k][j];
379             }
380         for (i = 0; i < 4; i++)
381             a[i][k] = a[i][k] / pivot;
382         a[k][k] = 1.0 / pivot;
383     }
384     for (k = 4 - 1; k >= 0; k--)
385         if (p[k] != k)
386             for (i = 0; i < 4; i++)
387             {
388                 h = a[i][k];
389                 a[i][k] = a[i][p[k]];
390                 a[i][p[k]] = h;
391             }
392 
393     for (i = 0; i < 4; i++)
394         for (j = 0; j < 4; j++)
395             inv.el[i][j] = a[i][j];
396     return (inv);
397 }
398 
399 Mat4    mat4_transpose(Mat4 m)
400 {
401     Mat4    trans = {Mat4_id};
402     int     i, j;
403 
404     for (i = 0; i < 4; ++i)
405         for (j = 0; j < 4; ++j)
406             trans.el[i][j] = m.el[j][i];
407     return (trans);
408 }
409 
410 double  mat4_trace(Mat4 m)
411 {
412     return (m.el[0][0] + m.el[1][1] + m.el[2][2] + m.el[3][3]);
413 }
414 
415 Vec4    mat4_vprod(Mat4 m, Vec4 v)
416 {
417     Vec4    prod = {Vec4_id};
418     double  sum;
419     int     i, j;
420 
421     for (i = 0; i < 4; ++i)
422     {
423         sum = 0.0;
424         for (j = 0; j < 4; ++j)
425             sum += m.el[i][j] * v.el[j];
426         prod.el[i] = sum;
427     }
428     return (prod);
429 }
430 
431 Vec4    mat4_transpose_vprod(Mat4 m, Vec4 v)
432 {
433     Vec4    prod = {Vec4_id};
434     double  sum;
435     int     i, j;
436 
437     for (i = 0; i < 4; ++i)
438     {
439         sum = 0.0;
440         for (j = 0; j < 4; ++j)
441             sum += m.el[j][i] * v.el[j];
442         prod.el[i] = sum;
443     }
444     return (prod);
445 }
446 
447 double  mat4_sprod(Vec4 v, Mat4 m, Vec4 w)
448 {
449     double  sum = 0.0;
450     int     i, j;
451 
452     for (i = 0; i < 4; ++i)
453         for (j = 0; j < 4; ++j)
454             sum += v.el[i] * m.el[i][j] * w.el[j];
455     return (sum);
456 }
457 
458 Mat4    mat4_tensor(Vec4 v, Vec4 w)
459 {
460     Mat4    prod = {Mat4_id};
461     int     i, j;
462 
463     for (i = 0; i < 4; ++i)
464         for (j = 0; j < 4; ++j)
465             prod.el[i][j] = v.el[i] * w.el[j];
466     return (prod);
467 }
468 
469 Mat4    mat4_read(FILE * fp)
470 {
471     Mat4    temp = {Mat4_id};
472     int     i;
473 
474     for (i = 0; i < 4; ++i)
475         (void) fscanf(fp, "%f %f %f %f",
476                       &temp.el[i][0], &temp.el[i][1], &temp.el[i][2], &temp.el[i][3]);
477     return (temp);
478 }
479 
480 void    mat4_print(FILE * fp, Mat4 m)
481 {
482     int     i;
483 
484     for (i = 0; i < 4; ++i)
485         (void) fprintf(fp, "%f %f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2], m.el[i][3]);
486 }
487 
488 void    mat4_pprint(FILE * fp, char *msg, Mat4 m)
489 {
490     int     i, n;
491 
492     n = strlen(msg);
493     (void) fprintf(fp, "%s", msg);
494     (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
495                    m.el[0][0], m.el[0][1], m.el[0][2], m.el[0][3]);
496     for (i = 0; i < n; ++i)
497         (void) fputc(' ', stderr);
498     (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
499                    m.el[1][0], m.el[1][1], m.el[1][2], m.el[1][3]);
500     for (i = 0; i < n; ++i)
501         (void) fputc(' ', stderr);
502     (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
503                    m.el[2][0], m.el[2][1], m.el[2][2], m.el[2][3]);
504     for (i = 0; i < n; ++i)
505         (void) fputc(' ', stderr);
506     (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
507                    m.el[3][0], m.el[3][1], m.el[3][2], m.el[3][3]);
508 }
509 
510 void    mat4_format(Mat4 m)
511 {
512     int     i;
513 
514     for (i = 0; i < 4; ++i)
515         format("%f %f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2], m.el[i][3]);
516 }
517 
518 void    mat4_eigen(Mat4 m, double *eval, Vec4 * evec)
519 {
520     int     i, j;
521     Matrix *a = matrix_alloc(4, 4, matrix_full, double_v);
522     Matrix *b = matrix_alloc(4, 4, matrix_full, double_v);
523     Vector *v = vector_alloc(4, double_v);
524 
525     /** copy mat4 into matrix **/
526     for (i = 0; i < 4; i++)
527         for (j = 0; j < 4; j++)
528             matrix_putf(m.el[i][j], a, i, j);
529 
530     /** get vector of eigenvals and matrix of column eigenvecs **/
531     (void) matrix_eigen_sym(a, v, b);
532 
533     /** copy into eval and evec **/
534     for (i = 0; i < 4; i++)
535         eval[i] = vector_getf(v, i);
536     for (i = 0; i < 4; i++)
537         for (j = 0; j < 4; j++)
538             evec[i].el[j] = matrix_getf(b, j, i);
539     matrix_free(a);
540     matrix_free(b);
541     vector_free(v);
542 }
543 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.