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

Linux Cross Reference
Tina6/tina-libs/tina/math/mathGeom_mat3.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_mat3.c,v $
 37  * Date    :  $Date: 2003/09/23 11:32:20 $
 38  * Version :  $Revision: 1.5 $
 39  * CVS Id  :  $Id: mathGeom_mat3.c,v 1.5 2003/09/23 11:32:20 matts Exp $
 40  *
 41  * Author  :  Legacy TINA
 42  *
 43  * Notes : Mat3 handling
 44  *
 45  *********
 46 */
 47 /** 
 48  *  @file
 49  *  @brief  Functions for handling 3 by 3 matrices (Mat3).      
 50  *
 51  *  Four main types of functions;
 52  *
 53  *  - Mat3 allocation and initialisation routines.
 54  *  - Functions for accessing and reordering the values in a Mat3.
 55  *  - Mat3 manipulations (eg addition of Mat3's, transpose of a Mat3, tensor of Mat3 etc).
 56  *  - Mat3 file/print/format routines.
 57  * 
 58 */
 59 #include "mathGeom_mat3.h"
 60 
 61 #if HAVE_CONFIG_H
 62 #include <config.h>
 63 #endif
 64 
 65 #include <math.h>
 66 #include <stdio.h>
 67 #include <string.h>
 68 #include <tina/sys/sysDef.h>
 69 #include <tina/sys/sysPro.h>
 70 #include <tina/math/math_GeomDef.h>
 71 #include <tina/math/math_MatrDef.h>
 72 #include <tina/math/math_MatrPro.h>
 73 #include <tina/math/math_VecDef.h>
 74 #include <tina/math/math_VecPro.h>
 75 #include <tina/math/mathGeom_vec3.h>
 76 #include <tina/math/mathNum_eigen.h>
 77 
 78 
 79 
 80 Mat3           *mat3_alloc(void)
 81 {
 82         Mat3           *m = ts_ralloc(Mat3);
 83 
 84         m->el[0][0] = 0.0;
 85         m->el[0][1] = 0.0;
 86         m->el[0][2] = 0.0;
 87         m->el[1][0] = 0.0;
 88         m->el[1][1] = 0.0;
 89         m->el[1][2] = 0.0;
 90         m->el[2][0] = 0.0;
 91         m->el[2][1] = 0.0;
 92         m->el[2][2] = 0.0;
 93         return (m);
 94 }
 95 
 96 Mat3           *mat3_make(Mat3 n)
 97 {
 98         Mat3           *m = ts_ralloc(Mat3);
 99 
100         *m = n;
101         return (m);
102 }
103 
104 void            mat3_free(Mat3 * m)
105 {
106         rfree((void *) m);
107 }
108 
109 /** function versions of component macros **/
110 
111 double          mat3_get_xx(Mat3 m)
112 {
113         return (mat3_xx(m));
114 }
115 
116 double          mat3_get_xy(Mat3 m)
117 {
118         return (mat3_xy(m));
119 }
120 
121 double          mat3_get_xz(Mat3 m)
122 {
123         return (mat3_xz(m));
124 }
125 
126 double          mat3_get_yx(Mat3 m)
127 {
128         return (mat3_yx(m));
129 }
130 
131 double          mat3_get_yy(Mat3 m)
132 {
133         return (mat3_yy(m));
134 }
135 
136 double          mat3_get_yz(Mat3 m)
137 {
138         return (mat3_yz(m));
139 }
140 
141 double          mat3_get_zx(Mat3 m)
142 {
143         return (mat3_zx(m));
144 }
145 
146 double          mat3_get_zy(Mat3 m)
147 {
148         return (mat3_zy(m));
149 }
150 
151 double          mat3_get_zz(Mat3 m)
152 {
153         return (mat3_zz(m));
154 }
155 
156 Mat3            mat3(double mxx, double mxy, double mxz, double myx, double myy, double myz, double mzx, double mzy, double mzz)
157 {
158         Mat3            m = {Mat3_id};
159 
160         m.el[0][0] = mxx;
161         m.el[0][1] = mxy;
162         m.el[0][2] = mxz;
163         m.el[1][0] = myx;
164         m.el[1][1] = myy;
165         m.el[1][2] = myz;
166         m.el[2][0] = mzx;
167         m.el[2][1] = mzy;
168         m.el[2][2] = mzz;
169         return (m);
170 }
171 
172 Mat3            mat3_unit(void)
173 {
174         return (mat3(1.0, 0.0, 0.0,
175                      0.0, 1.0, 0.0,
176                      0.0, 0.0, 1.0));
177 }
178 
179 Mat3            mat3_zero(void)
180 {
181         static Mat3     mat3_0 = {Mat3_id};             /* static data! */
182 
183         return (mat3_0);
184 }
185 
186 Mat3            mat3_diag(double mxx, double myy, double mzz)
187 {
188         return (mat3(mxx, 0.0, 0.0,
189                      0.0, myy, 0.0,
190                      0.0, 0.0, mzz));
191 }
192 
193 void            mat3_comps(Mat3 m, float *mxx, float *mxy, float *mxz, float *myx, float *myy, float *myz, float *mzx, float *mzy, float *mzz)
194 {
195         *mxx = m.el[0][0];
196         *mxy = m.el[0][1];
197         *mxz = m.el[0][2];
198         *myx = m.el[1][0];
199         *myy = m.el[1][1];
200         *myz = m.el[1][2];
201         *mzx = m.el[2][0];
202         *mzy = m.el[2][1];
203         *mzz = m.el[2][2];
204 }
205 
206 Vec3            mat3_rowx(Mat3 m)
207 {
208         return (vec3(m.el[0][0], m.el[0][1], m.el[0][2]));
209 }
210 
211 Vec3            mat3_rowy(Mat3 m)
212 {
213         return (vec3(m.el[1][0], m.el[1][1], m.el[1][2]));
214 }
215 
216 Vec3            mat3_rowz(Mat3 m)
217 {
218         return (vec3(m.el[2][0], m.el[2][1], m.el[2][2]));
219 }
220 
221 Vec3            mat3_colx(Mat3 m)
222 {
223         return (vec3(m.el[0][0], m.el[1][0], m.el[2][0]));
224 }
225 
226 Vec3            mat3_coly(Mat3 m)
227 {
228         return (vec3(m.el[0][1], m.el[1][1], m.el[2][1]));
229 }
230 
231 Vec3            mat3_colz(Mat3 m)
232 {
233         return (vec3(m.el[0][2], m.el[1][2], m.el[2][2]));
234 }
235 
236 Mat3            mat3_of_cols(Vec3 cx, Vec3 cy, Vec3 cz)
237 {
238         Mat3            m = {Mat3_id};
239 
240         m.el[0][0] = cx.el[0];
241         m.el[1][0] = cx.el[1];
242         m.el[2][0] = cx.el[2];
243         m.el[0][1] = cy.el[0];
244         m.el[1][1] = cy.el[1];
245         m.el[2][1] = cy.el[2];
246         m.el[0][2] = cz.el[0];
247         m.el[1][2] = cz.el[1];
248         m.el[2][2] = cz.el[2];
249         return (m);
250 }
251 
252 Mat3            mat3_of_rows(Vec3 rx, Vec3 ry, Vec3 rz)
253 {
254         Mat3            m = {Mat3_id};
255 
256         m.el[0][0] = rx.el[0];
257         m.el[0][1] = rx.el[1];
258         m.el[0][2] = rx.el[2];
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[2][0] = rz.el[0];
263         m.el[2][1] = rz.el[1];
264         m.el[2][2] = rz.el[2];
265         return (m);
266 }
267 
268 Mat3            mat3_sum(Mat3 m, Mat3 n)
269 {
270         Mat3            sum = {Mat3_id};
271         int             i, j;
272 
273         for (i = 0; i < 3; ++i)
274                 for (j = 0; j < 3; ++j)
275                         sum.el[i][j] = m.el[i][j] + n.el[i][j];
276         return (sum);
277 }
278 
279 Mat3            mat3_sum3(Mat3 l, Mat3 m, Mat3 n)
280 {
281         Mat3            sum = {Mat3_id};
282         int             i, j;
283 
284         for (i = 0; i < 3; ++i)
285                 for (j = 0; j < 3; ++j)
286                         sum.el[i][j] = l.el[i][j] + m.el[i][j] + n.el[i][j];
287         return (sum);
288 }
289 
290 Mat3            mat3_diff(Mat3 m, Mat3 n)
291 {
292         Mat3            diff = {Mat3_id};
293         int             i, j;
294 
295         for (i = 0; i < 3; ++i)
296                 for (j = 0; j < 3; ++j)
297                         diff.el[i][j] = m.el[i][j] - n.el[i][j];
298         return (diff);
299 }
300 
301 Mat3            mat3_minus(Mat3 m)
302 {
303         Mat3            minus = {Mat3_id};
304         int             i, j;
305 
306         for (i = 0; i < 3; ++i)
307                 for (j = 0; j < 3; ++j)
308                         minus.el[i][j] = -m.el[i][j];
309         return (minus);
310 }
311 
312 Mat3            mat3_times(double k, Mat3 m)
313 {
314         Mat3            prod = {Mat3_id};
315         int             i, j;
316 
317         for (i = 0; i < 3; ++i)
318                 for (j = 0; j < 3; ++j)
319                         prod.el[i][j] = k * m.el[i][j];
320         return (prod);
321 }
322 
323 Mat3            mat3_prod(Mat3 m, Mat3 n)
324 {
325         Mat3            prod = {Mat3_id};
326         double          sum;
327         int             i, j, k;
328 
329         for (i = 0; i < 3; ++i)
330                 for (j = 0; j < 3; ++j)
331                 {
332                         sum = 0.0;
333                         for (k = 0; k < 3; ++k)
334                                 sum += m.el[i][k] * n.el[k][j];
335                         prod.el[i][j] = sum;
336                 }
337         return (prod);
338 }
339 
340 Mat3            mat3_inverse(Mat3 m)
341 {
342         Mat3            inv = {Mat3_id};
343         double          a[3][3];
344         int             i, j, k, p[3];
345         double          h, q, s, sup, pivot;
346 
347         for (i = 0; i < 3; i++)
348                 for (j = 0; j < 3; j++)
349                         a[i][j] = m.el[i][j];
350 
351         for (k = 0; k < 3; k++)
352         {
353                 sup = 0.0;
354                 p[k] = 0;
355                 for (i = k; i < 3; i++)
356                 {
357                         s = 0.0;
358                         for (j = k; j < 3; j++)
359                                 s += fabs(a[i][j]);
360                         q = fabs(a[i][k]) / s;
361                         if (sup < q)
362                         {
363                                 sup = q;
364                                 p[k] = i;
365                         }
366                 }
367                 if (sup == 0.0)
368                         return (mat3_unit());
369                 if (p[k] != k)
370                         for (j = 0; j < 3; j++)
371                         {
372                                 h = a[k][j];
373                                 a[k][j] = a[p[k]][j];
374                                 a[p[k]][j] = h;
375                         }
376                 pivot = a[k][k];
377                 for (j = 0; j < 3; j++)
378                         if (j != k)
379                         {
380                                 a[k][j] = -a[k][j] / pivot;
381                                 for (i = 0; i < 3; i++)
382                                         if (i != k)
383                                                 a[i][j] += a[i][k] * a[k][j];
384                         }
385                 for (i = 0; i < 3; i++)
386                         a[i][k] = a[i][k] / pivot;
387                 a[k][k] = 1.0 / pivot;
388         }
389         for (k = 3 - 1; k >= 0; k--)
390                 if (p[k] != k)
391                         for (i = 0; i < 3; i++)
392                         {
393                                 h = a[i][k];
394                                 a[i][k] = a[i][p[k]];
395                                 a[i][p[k]] = h;
396                         }
397 
398         for (i = 0; i < 3; i++)
399                 for (j = 0; j < 3; j++)
400                         inv.el[i][j] = a[i][j];
401         return (inv);
402 }
403 
404 Mat3            mat3_transpose(Mat3 m)
405 {
406         Mat3            trans = {Mat3_id};
407         int             i, j;
408 
409         for (i = 0; i < 3; ++i)
410                 for (j = 0; j < 3; ++j)
411                         trans.el[i][j] = m.el[j][i];
412         return (trans);
413 }
414 
415 double          mat3_trace(Mat3 m)
416 {
417         return (m.el[0][0] + m.el[1][1] + m.el[2][2]);
418 }
419 
420 double          mat3_det(Mat3 m)
421 {
422         float           mxx, mxy, mxz, myx, myy, myz, mzx, mzy, mzz;
423 
424         mat3_comps(m, &mxx, &mxy, &mxz, &myx, &myy, &myz, &mzx, &mzy, &mzz);
425         return (mxx * myy * mzz + mxy * myz * mzx + mxz * myx * mzy
426                 - mxx * myz * mzy - mxy * myx * mzz - mxz * myy * mzx);
427 }
428 
429 Bool            mat3_posdef(Mat3 m)
430 {
431         if (m.el[0][0] <= 0.0)
432                 return (false);
433         else if (m.el[0][0] * m.el[1][1] - m.el[0][1] * m.el[1][0] <= 0.0)
434                 return (false);
435         else if (mat3_det(m) <= 0.0)
436                 return (false);
437         else
438                 return (true);
439 }
440 
441 Vec3            mat3_vprod(Mat3 m, Vec3 v)
442 {
443         Vec3            prod = {Vec3_id};
444         double          sum;
445         int             i, j;
446 
447         for (i = 0; i < 3; ++i)
448         {
449                 sum = 0.0;
450                 for (j = 0; j < 3; ++j)
451                         sum += m.el[i][j] * v.el[j];
452                 prod.el[i] = sum;
453         }
454         return (prod);
455 }
456 
457 Vec3            mat3_transpose_vprod(Mat3 m, Vec3 v)
458 {
459         Vec3            prod = {Vec3_id};
460         double          sum;
461         int             i, j;
462 
463         for (i = 0; i < 3; ++i)
464         {
465                 sum = 0.0;
466                 for (j = 0; j < 3; ++j)
467                         sum += m.el[j][i] * v.el[j];
468                 prod.el[i] = sum;
469         }
470         return (prod);
471 }
472 
473 double          mat3_sprod(Vec3 v, Mat3 m, Vec3 w)
474 {
475         double          sum = 0.0;
476         int             i, j;
477 
478         for (i = 0; i < 3; ++i)
479                 for (j = 0; j < 3; ++j)
480                         sum += v.el[i] * m.el[i][j] * w.el[j];
481         return (sum);
482 }
483 
484 Mat3            mat3_tensor(Vec3 v, Vec3 w)
485 {
486         Mat3            prod = {Mat3_id};
487         int             i, j;
488 
489         for (i = 0; i < 3; ++i)
490                 for (j = 0; j < 3; ++j)
491                         prod.el[i][j] = v.el[i] * w.el[j];
492         return (prod);
493 }
494 
495 Mat3            mat3_sum_tensor(Mat3 m, Vec3 v, Vec3 w)
496 {
497         Mat3            sum = {Mat3_id};
498         int             i, j;
499 
500         for (i = 0; i < 3; ++i)
501                 for (j = 0; j < 3; ++j)
502                         sum.el[i][j] = m.el[i][j] + v.el[i] * w.el[j];
503         return (sum);
504 }
505 
506 Mat3            mat3_read(FILE * fp)
507 {
508         Mat3            temp = {Mat3_id};
509         int             i;
510 
511         for (i = 0; i < 3; ++i)
512                 (void) fscanf(fp, "%f %f %f",
513                             &temp.el[i][0], &temp.el[i][1], &temp.el[i][2]);
514         return (temp);
515 }
516 
517 void            mat3_print(FILE * fp, Mat3 m)
518 {
519         int             i;
520 
521         for (i = 0; i < 3; ++i)
522                 (void) fprintf(fp, "%f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2]);
523 }
524 
525 void            mat3_pprint(FILE * fp, char *msg, Mat3 m)
526 {
527         int             i, n;
528 
529         n = strlen(msg);
530         (void) fprintf(fp, "%s", msg);
531         (void) fprintf(fp, "|%15.6f%15.6f%15.6f|\n",
532                        m.el[0][0], m.el[0][1], m.el[0][2]);
533         for (i = 0; i < n; ++i)
534                 (void) fprintf(fp, " ");
535         (void) fprintf(fp, "|%15.6f%15.6f%15.6f|\n",
536                        m.el[1][0], m.el[1][1], m.el[1][2]);
537         for (i = 0; i < n; ++i)
538                 (void) fprintf(fp, " ");
539         (void) fprintf(fp, "|%15.6f%15.6f%15.6f|\n",
540                        m.el[2][0], m.el[2][1], m.el[2][2]);
541 }
542 
543 void            mat3_format(Mat3 m)
544 {
545         int             i;
546 
547         for (i = 0; i < 3; ++i)
548                 format("%f %f %f\n", m.el[i][0], m.el[i][1], m.el[i][2]);
549 }
550 
551 #if 0
552 /* ICCBUG static function: order never called */
553 static void     order(x1, x2, x3)
554         double         *x1, *x2, *x3;
555 {
556         double          y1 = *x1;
557         double          y2 = *x2;
558         double          y3 = *x3;
559 
560         if (y1 <= y2)
561         {
562                 if (y3 <= y1)
563                 {
564                         *x1 = y3;
565                         *x2 = y1;
566                         *x3 = y2;
567                 } else if (y3 <= y2)
568                 {
569                         *x1 = y1;
570                         *x2 = y3;
571                         *x3 = y2;
572                 } else
573                 {
574                         *x1 = y1;
575                         *x2 = y2;
576                         *x3 = y3;
577                 }
578         } else
579         {
580                 if (y3 <= y2)
581                 {
582                         *x1 = y3;
583                         *x2 = y2;
584                         *x3 = y1;
585                 } else if (y3 <= y1)
586                 {
587                         *x1 = y2;
588                         *x2 = y3;
589                         *x3 = y1;
590                 } else
591                 {
592                         *x1 = y2;
593                         *x2 = y1;
594                         *x3 = y3;
595                 }
596         }
597 }
598 
599 #endif
600 
601 /************
602 
603 Vec3            mat3_eigenvec(m, l)
604 Mat3            m = {Mat3_id};
605 double          l;
606 {
607     Vec3            rx = {Vec3_id};
608 Vec3 ry = {Vec3_id};
609 Vec3 rz = {Vec3_id};
610     Vec3            ex = {Vec3_id};
611 Vec3 ey = {Vec3_id};
612 Vec3 ez = {Vec3_id};
613     double          nx, ny, nz, n;
614 
615     rx = vec3_unit(vec3_diff(mat3_rowx(m), vec3(l, 0.0, 0.0)));
616     ry = vec3_unit(vec3_diff(mat3_rowy(m), vec3(0.0, l, 0.0)));
617     rz = vec3_unit(vec3_diff(mat3_rowz(m), vec3(0.0, 0.0, l)));
618 
619     ex = vec3_cross(ry, rz);
620     nx = vec3_mod(ex);
621     ey = vec3_cross(rz, rx);
622     ny = vec3_mod(ey);
623     ez = vec3_cross(rx, ry);
624     nz = vec3_mod(ez);
625     n = MAX3(nx, ny, nz);
626     if (n == nx)
627         return (vec3_unit(ex));
628     else if (n == ny)
629         return (vec3_unit(ey));
630     else
631         return (vec3_unit(ez));
632 }
633 
634 Bool            mat3_eigen(m, l1, l2, l3, e1, e2, e3)
635 Mat3            m = {Mat3_id};
636 double         *l1, *l2, *l3;
637 Vec3           *e1;
638 Vec3 *e2;
639 Vec3 *e3;
640 {
641     double          mxx = mat3_xx(m);
642     double          mxy = mat3_xy(m);
643     double          mxz = mat3_xz(m);
644     double          myx = mat3_yx(m);
645     double          myy = mat3_yy(m);
646     double          myz = mat3_yz(m);
647     double          mzx = mat3_zx(m);
648     double          mzy = mat3_zy(m);
649     double          mzz = mat3_zz(m);
650     double          a, b, c, d;
651     Bool            rl;
652 
653     a = -1.0;
654     b = mxx + myy + mzz;
655     c = -(myy * mzz - myz * mzy) - (mxx * mzz - mxz * mzx) - (mxx * myy - mxy * myx);
656     d = mat3_det(m);
657 
658     if (!cubic_roots(a, b, c, d, l1, l2, l3))
659         return (false);
660 
661     order(l1, l2, l3);
662     *e1 = mat3_eigenvec(m, *l1);
663     *e2 = mat3_eigenvec(m, *l2);
664     *e3 = mat3_eigenvec(m, *l3);
665 }
666 
667 *************/
668 
669 void            mat3_eigen(Mat3 m, double *eval, Vec3 * evec)
670 {
671         int             i, j;
672         Matrix         *a = matrix_alloc(3, 3, matrix_full, double_v);
673         Matrix         *b = matrix_alloc(3, 3, matrix_full, double_v);
674         Vector         *v = vector_alloc(3, double_v);
675 
676         /** copy mat3 into matrix **/
677         for (i = 0; i < 3; i++)
678                 for (j = 0; j < 3; j++)
679                         matrix_putf(m.el[i][j], a, i, j);
680 
681         /** get vector of eigenvals and matrix of column eigenvecs **/
682         (void) matrix_eigen_sym(a, v, b);
683 
684         /** copy into eval and evec **/
685         for (i = 0; i < 3; i++)
686                 eval[i] = vector_getf(v, i);
687         for (i = 0; i < 3; i++)
688                 for (j = 0; j < 3; j++)
689                         evec[i].el[j] = matrix_getf(b, j, i);
690         matrix_free(a);
691         matrix_free(b);
692         vector_free(v);
693 }
694 

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