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

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

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

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

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