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

Linux Cross Reference
Tina6/tina-libs/tina/math/mathGeom_mat4.c

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

  1 /**********
  2  *
  3  * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
  4  * University of Manchester, UK.  All rights reserved.
  5  * 
  6  * Redistribution and use in source and binary forms, with or without modification, 
  7  * are permitted provided that the following conditions are met:
  8  * 
  9  *   . Redistributions of source code must retain the above copyright notice, 
 10  *     this list of conditions and the following disclaimer.
 11  *    
 12  *   . Redistributions in binary form must reproduce the above copyright notice,
 13  *     this list of conditions and the following disclaimer in the documentation 
 14  *     and/or other materials provided with the distribution.
 15  * 
 16  *   . Neither the name of the University of Manchester nor the names of its
 17  *     contributors may be used to endorse or promote products derived from this 
 18  *     software without specific prior written permission.
 19  * 
 20  * 
 21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  * POSSIBILITY OF SUCH DAMAGE.
 32  *
 33  **********
 34  *
 35  * Program :    TINA
 36  * File    :  $Source: /home/tina/cvs/tina-libs/tina/math/mathGeom_mat4.c,v $
 37  * Date    :  $Date: 2003/09/23 11:32:20 $
 38  * Version :  $Revision: 1.5 $
 39  * CVS Id  :  $Id: mathGeom_mat4.c,v 1.5 2003/09/23 11:32:20 matts Exp $
 40  *
 41  * Author  :  Legacy TINA
 42  *
 43  * Notes : Mat4 handling
 44  *
 45  *********
 46 */
 47 /** 
 48  *  @file
 49  *  @brief  Functions for handling 4 by 4 matrices (Mat4).      
 50  *
 51  *  Four main types of functions;
 52  *
 53  *  - Mat4 allocation and initialisation routines.
 54  *  - Functions for accessing and reordering the values in a Mat4.
 55  *  - Mat4 manipulations (eg addition of Mat4's, transpose of a Mat4, tensor of Mat4 etc).
 56  *  - Mat4 file/print/format routines.
 57  * 
 58 */
 59 #include "mathGeom_mat4.h"
 60 
 61 #if HAVE_CONFIG_H
 62 #include <config.h>
 63 #endif
 64 
 65 
 66 #include <stdio.h>
 67 #include <string.h>
 68 #include <math.h>
 69 #include <tina/sys/sysDef.h>
 70 #include <tina/sys/sysPro.h>
 71 #include <tina/math/math_GeomDef.h>
 72 #include <tina/math/math_MatrDef.h>
 73 #include <tina/math/math_MatrPro.h>
 74 #include <tina/math/math_VecDef.h>
 75 #include <tina/math/math_VecPro.h>
 76 #include <tina/math/mathGeom_vec4.h>
 77 #include <tina/math/mathNum_eigen.h>
 78 
 79 
 80 
 81 Mat4           *mat4_alloc(void)
 82 {
 83         Mat4           *m = ts_ralloc(Mat4);
 84 
 85         m->el[0][0] = 0.0;
 86         m->el[0][1] = 0.0;
 87         m->el[0][2] = 0.0;
 88         m->el[0][3] = 0.0;
 89         m->el[1][0] = 0.0;
 90         m->el[1][1] = 0.0;
 91         m->el[1][2] = 0.0;
 92         m->el[1][3] = 0.0;
 93         m->el[2][0] = 0.0;
 94         m->el[2][1] = 0.0;
 95         m->el[2][2] = 0.0;
 96         m->el[2][3] = 0.0;
 97         m->el[3][0] = 0.0;
 98         m->el[3][1] = 0.0;
 99         m->el[3][2] = 0.0;
100         m->el[3][3] = 0.0;
101         return (m);
102 }
103 
104 Mat4           *mat4_make(Mat4 n)
105 {
106         Mat4           *m = ts_ralloc(Mat4);
107 
108         *m = n;
109         return (m);
110 }
111 
112 void            mat4_free(Mat4 * m)
113 {
114         rfree((void *) m);
115 }
116 
117 /** function versions of component macros **/
118 
119 double          mat4_get_xx(Mat4 m)
120 {
121         return (mat4_xx(m));
122 }
123 
124 double          mat4_get_xy(Mat4 m)
125 {
126         return (mat4_xy(m));
127 }
128 
129 double          mat4_get_xz(Mat4 m)
130 {
131         return (mat4_xz(m));
132 }
133 
134 double          mat4_get_xw(Mat4 m)
135 {
136         return (mat4_xw(m));
137 }
138 
139 double          mat4_get_yx(Mat4 m)
140 {
141         return (mat4_yx(m));
142 }
143 
144 double          mat4_get_yy(Mat4 m)
145 {
146         return (mat4_yy(m));
147 }
148 
149 double          mat4_get_yz(Mat4 m)
150 {
151         return (mat4_yz(m));
152 }
153 
154 double          mat4_get_yw(Mat4 m)
155 {
156         return (mat4_yw(m));
157 }
158 
159 double          mat4_get_zx(Mat4 m)
160 {
161         return (mat4_zx(m));
162 }
163 
164 double          mat4_get_zy(Mat4 m)
165 {
166         return (mat4_zy(m));
167 }
168 
169 double          mat4_get_zz(Mat4 m)
170 {
171         return (mat4_zz(m));
172 }
173 
174 double          mat4_get_zw(Mat4 m)
175 {
176         return (mat4_zw(m));
177 }
178 
179 double          mat4_get_wx(Mat4 m)
180 {
181         return (mat4_wx(m));
182 }
183 
184 double          mat4_get_wy(Mat4 m)
185 {
186         return (mat4_wy(m));
187 }
188 
189 double          mat4_get_wz(Mat4 m)
190 {
191         return (mat4_wz(m));
192 }
193 
194 double          mat4_get_ww(Mat4 m)
195 {
196         return (mat4_ww(m));
197 }
198 
199 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)
200 {
201         Mat4            m = {Mat4_id};
202 
203         m.el[0][0] = mxx;
204         m.el[0][1] = mxy;
205         m.el[0][2] = mxz;
206         m.el[0][3] = mxw;
207         m.el[1][0] = myx;
208         m.el[1][1] = myy;
209         m.el[1][2] = myz;
210         m.el[1][3] = myw;
211         m.el[2][0] = mzx;
212         m.el[2][1] = mzy;
213         m.el[2][2] = mzz;
214         m.el[2][3] = mzw;
215         m.el[3][0] = mwx;
216         m.el[3][1] = mwy;
217         m.el[3][2] = mwz;
218         m.el[3][3] = mww;
219         return (m);
220 }
221 
222 Mat4            mat4_unit(void)
223 {
224         return (mat4(1.0, 0.0, 0.0, 0.0,
225                      0.0, 1.0, 0.0, 0.0,
226                      0.0, 0.0, 1.0, 0.0,
227                      0.0, 0.0, 0.0, 1.0));
228 }
229 
230 Mat4            mat4_zero(void)
231 {
232         static Mat4     mat4_0 = {Mat4_id};                     /* static data! */
233 
234         return (mat4_0);
235 }
236 
237 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)
238 {
239         *mxx = m.el[0][0];
240         *mxy = m.el[0][1];
241         *mxz = m.el[0][2];
242         *mxw = m.el[0][3];
243         *myx = m.el[1][0];
244         *myy = m.el[1][1];
245         *myz = m.el[1][2];
246         *myw = m.el[1][3];
247         *mzx = m.el[2][0];
248         *mzy = m.el[2][1];
249         *mzz = m.el[2][2];
250         *mzw = m.el[2][3];
251         *mwx = m.el[3][0];
252         *mwy = m.el[3][1];
253         *mwz = m.el[3][2];
254         *mww = m.el[3][3];
255 }
256 
257 Vec4            mat4_rowx(Mat4 m)
258 {
259         return (vec4(m.el[0][0], m.el[0][1], m.el[0][2], m.el[0][3]));
260 }
261 
262 Vec4            mat4_rowy(Mat4 m)
263 {
264         return (vec4(m.el[1][0], m.el[1][1], m.el[1][2], m.el[1][3]));
265 }
266 
267 Vec4            mat4_rowz(Mat4 m)
268 {
269         return (vec4(m.el[2][0], m.el[2][1], m.el[2][2], m.el[2][3]));
270 }
271 
272 Vec4            mat4_roww(Mat4 m)
273 {
274         return (vec4(m.el[3][0], m.el[3][1], m.el[3][2], m.el[3][3]));
275 }
276 
277 Vec4            mat4_colx(Mat4 m)
278 {
279         return (vec4(m.el[0][0], m.el[1][0], m.el[2][0], m.el[3][0]));
280 }
281 
282 Vec4            mat4_coly(Mat4 m)
283 {
284         return (vec4(m.el[0][1], m.el[1][1], m.el[2][1], m.el[3][1]));
285 }
286 
287 Vec4            mat4_colz(Mat4 m)
288 {
289         return (vec4(m.el[0][2], m.el[1][2], m.el[2][2], m.el[3][2]));
290 }
291 
292 Vec4            mat4_colw(Mat4 m)
293 {
294         return (vec4(m.el[0][3], m.el[1][3], m.el[2][3], m.el[3][3]));
295 }
296 
297 Mat4            mat4_of_cols(Vec4 cx, Vec4 cy, Vec4 cz, Vec4 cw)
298 {
299         Mat4            m = {Mat4_id};
300 
301         m.el[0][0] = cx.el[0];
302         m.el[1][0] = cx.el[1];
303         m.el[2][0] = cx.el[2];
304         m.el[3][0] = cx.el[3];
305         m.el[0][1] = cy.el[0];
306         m.el[1][1] = cy.el[1];
307         m.el[2][1] = cy.el[2];
308         m.el[3][1] = cy.el[3];
309         m.el[0][2] = cz.el[0];
310         m.el[1][2] = cz.el[1];
311         m.el[2][2] = cz.el[2];
312         m.el[3][2] = cz.el[3];
313         m.el[0][3] = cw.el[0];
314         m.el[1][3] = cw.el[1];
315         m.el[2][3] = cw.el[2];
316         m.el[3][3] = cw.el[3];
317         return (m);
318 }
319 
320 Mat4            mat4_of_rows(Vec4 rx, Vec4 ry, Vec4 rz, Vec4 rw)
321 {
322         Mat4            m = {Mat4_id};
323 
324         m.el[0][0] = rx.el[0];
325         m.el[0][1] = rx.el[1];
326         m.el[0][2] = rx.el[2];
327         m.el[0][3] = rx.el[3];
328         m.el[1][0] = ry.el[0];
329         m.el[1][1] = ry.el[1];
330         m.el[1][2] = ry.el[2];
331         m.el[1][3] = ry.el[3];
332         m.el[2][0] = rz.el[0];
333         m.el[2][1] = rz.el[1];
334         m.el[2][2] = rz.el[2];
335         m.el[2][3] = rz.el[3];
336         m.el[3][0] = rw.el[0];
337         m.el[3][1] = rw.el[1];
338         m.el[3][2] = rw.el[2];
339         m.el[3][3] = rw.el[3];
340         return (m);
341 }
342 
343 Mat4            mat4_sum(Mat4 m, Mat4 n)
344 {
345         Mat4            sum = {Mat4_id};
346         int             i, j;
347 
348         for (i = 0; i < 4; ++i)
349                 for (j = 0; j < 4; ++j)
350                         sum.el[i][j] = m.el[i][j] + n.el[i][j];
351         return (sum);
352 }
353 
354 Mat4            mat4_diff(Mat4 m, Mat4 n)
355 {
356         Mat4            diff = {Mat4_id};
357         int             i, j;
358 
359         for (i = 0; i < 4; ++i)
360                 for (j = 0; j < 4; ++j)
361                         diff.el[i][j] = m.el[i][j] - n.el[i][j];
362         return (diff);
363 }
364 
365 Mat4            mat4_minus(Mat4 m)
366 {
367         Mat4            minus = {Mat4_id};
368         int             i, j;
369 
370         for (i = 0; i < 4; ++i)
371                 for (j = 0; j < 4; ++j)
372                         minus.el[i][j] = -m.el[i][j];
373         return (minus);
374 }
375 
376 Mat4            mat4_times(double k, Mat4 m)
377 {
378         Mat4            prod = {Mat4_id};
379         int             i, j;
380 
381         for (i = 0; i < 4; ++i)
382                 for (j = 0; j < 4; ++j)
383                         prod.el[i][j] = k * m.el[i][j];
384         return (prod);
385 }
386 
387 Mat4            mat4_prod(Mat4 m, Mat4 n)
388 {
389         Mat4            prod = {Mat4_id};
390         double          sum;
391         int             i, j, k;
392 
393         for (i = 0; i < 4; ++i)
394                 for (j = 0; j < 4; ++j)
395                 {
396                         sum = 0.0;
397                         for (k = 0; k < 4; ++k)
398                                 sum += m.el[i][k] * n.el[k][j];
399                         prod.el[i][j] = sum;
400                 }
401         return (prod);
402 }
403 
404 Mat4            mat4_inverse(Mat4 m)
405 {
406         Mat4            inv = {Mat4_id};
407         double          a[4][4];
408         int             i, j, k, p[4];
409         double          h, q, s, sup, pivot;
410 
411         for (i = 0; i < 4; i++)
412                 for (j = 0; j < 4; j++)
413                         a[i][j] = m.el[i][j];
414 
415         for (k = 0; k < 4; k++)
416         {
417                 sup = 0.0;
418                 p[k] = 0;
419                 for (i = k; i < 4; i++)
420                 {
421                         s = 0.0;
422                         for (j = k; j < 4; j++)
423                                 s += fabs(a[i][j]);
424                         q = fabs(a[i][k]) / s;
425                         if (sup < q)
426                         {
427                                 sup = q;
428                                 p[k] = i;
429                         }
430                 }
431                 if (sup == 0.0)
432                         return (mat4_unit());
433                 if (p[k] != k)
434                         for (j = 0; j < 4; j++)
435                         {
436                                 h = a[k][j];
437                                 a[k][j] = a[p[k]][j];
438                                 a[p[k]][j] = h;
439                         }
440                 pivot = a[k][k];
441                 for (j = 0; j < 4; j++)
442                         if (j != k)
443                         {
444                                 a[k][j] = -a[k][j] / pivot;
445                                 for (i = 0; i < 4; i++)
446                                         if (i != k)
447                                                 a[i][j] += a[i][k] * a[k][j];
448                         }
449                 for (i = 0; i < 4; i++)
450                         a[i][k] = a[i][k] / pivot;
451                 a[k][k] = 1.0 / pivot;
452         }
453         for (k = 4 - 1; k >= 0; k--)
454                 if (p[k] != k)
455                         for (i = 0; i < 4; i++)
456                         {
457                                 h = a[i][k];
458                                 a[i][k] = a[i][p[k]];
459                                 a[i][p[k]] = h;
460                         }
461 
462         for (i = 0; i < 4; i++)
463                 for (j = 0; j < 4; j++)
464                         inv.el[i][j] = a[i][j];
465         return (inv);
466 }
467 
468 Mat4            mat4_transpose(Mat4 m)
469 {
470         Mat4            trans = {Mat4_id};
471         int             i, j;
472 
473         for (i = 0; i < 4; ++i)
474                 for (j = 0; j < 4; ++j)
475                         trans.el[i][j] = m.el[j][i];
476         return (trans);
477 }
478 
479 double          mat4_trace(Mat4 m)
480 {
481         return (m.el[0][0] + m.el[1][1] + m.el[2][2] + m.el[3][3]);
482 }
483 
484 Vec4            mat4_vprod(Mat4 m, Vec4 v)
485 {
486         Vec4            prod = {Vec4_id};
487         double          sum;
488         int             i, j;
489 
490         for (i = 0; i < 4; ++i)
491         {
492                 sum = 0.0;
493                 for (j = 0; j < 4; ++j)
494                         sum += m.el[i][j] * v.el[j];
495                 prod.el[i] = sum;
496         }
497         return (prod);
498 }
499 
500 Vec4            mat4_transpose_vprod(Mat4 m, Vec4 v)
501 {
502         Vec4            prod = {Vec4_id};
503         double          sum;
504         int             i, j;
505 
506         for (i = 0; i < 4; ++i)
507         {
508                 sum = 0.0;
509                 for (j = 0; j < 4; ++j)
510                         sum += m.el[j][i] * v.el[j];
511                 prod.el[i] = sum;
512         }
513         return (prod);
514 }
515 
516 double          mat4_sprod(Vec4 v, Mat4 m, Vec4 w)
517 {
518         double          sum = 0.0;
519         int             i, j;
520 
521         for (i = 0; i < 4; ++i)
522                 for (j = 0; j < 4; ++j)
523                         sum += v.el[i] * m.el[i][j] * w.el[j];
524         return (sum);
525 }
526 
527 Mat4            mat4_tensor(Vec4 v, Vec4 w)
528 {
529         Mat4            prod = {Mat4_id};
530         int             i, j;
531 
532         for (i = 0; i < 4; ++i)
533                 for (j = 0; j < 4; ++j)
534                         prod.el[i][j] = v.el[i] * w.el[j];
535         return (prod);
536 }
537 
538 Mat4            mat4_read(FILE * fp)
539 {
540         Mat4            temp = {Mat4_id};
541         int             i;
542 
543         for (i = 0; i < 4; ++i)
544                 (void) fscanf(fp, "%f %f %f %f",
545                               &temp.el[i][0], &temp.el[i][1], &temp.el[i][2], &temp.el[i][3]);
546         return (temp);
547 }
548 
549 void            mat4_print(FILE * fp, Mat4 m)
550 {
551         int             i;
552 
553         for (i = 0; i < 4; ++i)
554                 (void) fprintf(fp, "%f %f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2], m.el[i][3]);
555 }
556 
557 void            mat4_pprint(FILE * fp, char *msg, Mat4 m)
558 {
559         int             i, n;
560 
561         n = strlen(msg);
562         (void) fprintf(fp, "%s", msg);
563         (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
564                        m.el[0][0], m.el[0][1], m.el[0][2], m.el[0][3]);
565         for (i = 0; i < n; ++i)
566                 (void) fputc(' ', stderr);
567         (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
568                        m.el[1][0], m.el[1][1], m.el[1][2], m.el[1][3]);
569         for (i = 0; i < n; ++i)
570                 (void) fputc(' ', stderr);
571         (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
572                        m.el[2][0], m.el[2][1], m.el[2][2], m.el[2][3]);
573         for (i = 0; i < n; ++i)
574                 (void) fputc(' ', stderr);
575         (void) fprintf(fp, "|%15.6f%15.6f%15.6f%15.6f|\n",
576                        m.el[3][0], m.el[3][1], m.el[3][2], m.el[3][3]);
577 }
578 
579 void            mat4_format(Mat4 m)
580 {
581         int             i;
582 
583         for (i = 0; i < 4; ++i)
584                 format("%f %f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2], m.el[i][3]);
585 }
586 
587 void            mat4_eigen(Mat4 m, double *eval, Vec4 * evec)
588 {
589         int             i, j;
590         Matrix         *a = matrix_alloc(4, 4, matrix_full, double_v);
591         Matrix         *b = matrix_alloc(4, 4, matrix_full, double_v);
592         Vector         *v = vector_alloc(4, double_v);
593 
594         /** copy mat4 into matrix **/
595         for (i = 0; i < 4; i++)
596                 for (j = 0; j < 4; j++)
597                         matrix_putf(m.el[i][j], a, i, j);
598 
599         /** get vector of eigenvals and matrix of column eigenvecs **/
600         (void) matrix_eigen_sym(a, v, b);
601 
602         /** copy into eval and evec **/
603         for (i = 0; i < 4; i++)
604                 eval[i] = vector_getf(v, i);
605         for (i = 0; i < 4; i++)
606                 for (j = 0; j < 4; j++)
607                         evec[i].el[j] = matrix_getf(b, j, i);
608         matrix_free(a);
609         matrix_free(b);
610         vector_free(v);
611 }
612 

~ [ 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.