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

Linux Cross Reference
Tina4/src/math/transform/rot3.c

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

  1 /**@(#)Rotation
  2 **/
  3 #include <stdio.h>
  4 #include <math.h>
  5 #include <tina/sys.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 
  9 Mat3    rot3(double theta, Vec3 axis)
 10 {
 11     double  c, c1, s, a;
 12     float   nx, ny, nz;
 13     double  mxx, mxy, mxz;
 14     double  myx, myy, myz;
 15     double  mzx, mzy, mzz;
 16 
 17     a = vec3_mod(axis);
 18     if (a == 0.0)
 19         return (mat3_unit());
 20     vec3_comps(vec3_unit(axis), &nx, &ny, &nz);
 21     c = cos(theta);
 22     s = sin(theta);
 23     c1 = 1.0 - c;
 24     mxx = c + nx * nx * c1;
 25     mxy = nx * ny * c1 - nz * s;
 26     mxz = nx * nz * c1 + ny * s;
 27     myx = ny * nx * c1 + nz * s;
 28     myy = c + ny * ny * c1;
 29     myz = ny * nz * c1 - nx * s;
 30     mzx = nz * nx * c1 - ny * s;
 31     mzy = nz * ny * c1 + nx * s;
 32     mzz = c + nz * nz * c1;
 33     return (mat3(mxx, mxy, mxz,
 34                  myx, myy, myz,
 35                  mzx, mzy, mzz));
 36 }
 37 
 38 Mat3    rot3_1(Vec3 axis)
 39 {
 40     double  theta;
 41     double  c, c1, s;
 42     float   nx, ny, nz;
 43     double  mxx, mxy, mxz;
 44     double  myx, myy, myz;
 45     double  mzx, mzy, mzz;
 46 
 47     theta = vec3_mod(axis);
 48     if (theta == 0.0)
 49         return (mat3_unit());
 50     vec3_comps(vec3_unit(axis), &nx, &ny, &nz);
 51     c = cos(theta);
 52     s = sin(theta);
 53     c1 = 1.0 - c;
 54     mxx = c + nx * nx * c1;
 55     mxy = nx * ny * c1 - nz * s;
 56     mxz = nx * nz * c1 + ny * s;
 57     myx = ny * nx * c1 + nz * s;
 58     myy = c + ny * ny * c1;
 59     myz = ny * nz * c1 - nx * s;
 60     mzx = nz * nx * c1 - ny * s;
 61     mzy = nz * ny * c1 + nx * s;
 62     mzz = c + nz * nz * c1;
 63     return (mat3(mxx, mxy, mxz,
 64                  myx, myy, myz,
 65                  mzx, mzy, mzz));
 66 }
 67 
 68 void    rot3_angle_axis(Mat3 m, double *theta, Vec3 * axis)
 69 {
 70     double  c;
 71     Vec3    v = {Vec3_id};
 72 
 73     c = 0.5 * (mat3_trace(m) - 1.0);
 74     v = vec3_unit(vec3(mat3_zy(m) - mat3_yz(m),
 75                        mat3_xz(m) - mat3_zx(m),
 76                        mat3_yx(m) - mat3_xy(m)));
 77     if (theta == NULL)
 78         *axis = vec3_times(tina_acos(c), v);
 79     else
 80     {
 81         *axis = v;
 82         *theta = tina_acos(c);
 83     }
 84 }
 85 
 86 Mat3    rot3_euler(double theta, double phi, double psi)
 87 {
 88     Mat3    r1 = {Mat3_id};
 89     Mat3    r2 = {Mat3_id};
 90     Mat3    r3 = {Mat3_id};
 91 
 92     r1 = rot3(theta, vec3_ey());
 93     r2 = rot3(phi, vec3_ez());
 94     r3 = rot3(psi, vec3_ez());
 95     return (mat3_prod(r2, mat3_prod(r1, r3)));
 96 }
 97 

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