home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 1997 March / VPR9703A.ISO / VPR_DATA / DOGA / SOURCES / REND.LZH / READER / MATRIX.C < prev    next >
C/C++ Source or Header  |  1995-11-07  |  6KB  |  364 lines
  1. /*
  2.         行列演算関数群
  3.  
  4.         Copyright T.Kobayashi
  5. */
  6. #ifndef INLINE_MATRIX
  7.     #include <stdio.h>
  8.     #include <math.h>
  9.     #include <ctype.h>
  10.     #include <string.h>
  11.  
  12.     #include "reader.h"
  13.     #define    Static
  14. #else
  15.     extern  Float minscale;
  16.     #define Static static
  17. #endif
  18. #ifdef V70
  19. #define RAD 0.0174532925199
  20. #else
  21. #define RAD (3.14159265358979/180.0)
  22. #endif
  23.  
  24. /*    ベクトルの表示    */
  25. #if 1
  26. Static inline void    v_print( msg, v )
  27. char    *msg ;
  28. Vector    v ;
  29. {
  30. #if 0
  31.     fprintf( errfp, "%s\t%18g\t18g\t%18g\n", msg, v[0], v[1], v[2] ) ;
  32. #endif
  33.     fprintf( errfp, "%s\t%18lf\t%18lf\t%18lf\n", msg, v[0], v[1], v[2] ) ;
  34. }
  35.  
  36. /*    行列の表示  */
  37. Static inline void    m_print( m )
  38. Matrix    m ;
  39. {
  40.     int        i, j ;
  41.  
  42.     for( i = 0 ; i < 4 ; ++i )
  43.     {
  44.         for( j = 0 ; j < 4 ; ++j )
  45.             fprintf( errfp, "%18g\t", m[i][j] );
  46.         putchar( '\n' ) ;
  47.     }
  48. }
  49. #endif
  50.  
  51. /*    行列の複写    */
  52. Static inline void    m_copy( a, b )
  53. Matrix    a, b ;
  54. {
  55. #if 0
  56.     int i, j ;
  57.     for( i = 0 ; i < 4 ; ++i )
  58.         for( j = 0 ; j < 4 ; ++j )
  59.             a[i][j] = b[i][j] ;
  60. #endif
  61.     memcpy(a, b, sizeof(Matrix));
  62. }
  63.  
  64. /*
  65.  *        行列
  66.  */
  67. #ifdef V70
  68. static    Matrix    unit_matrix = {
  69.     1.0, 0.0, 0.0, 0.0,
  70.     0.0, 1.0, 0.0, 0.0,
  71.     0.0, 0.0, 1.0, 0.0,
  72.     0.0, 0.0, 0.0, 1.0
  73. } ;
  74. #endif
  75. /*    単位行列    */
  76. Static inline void    m_unit( a )
  77. Matrix    a ;
  78. {
  79. #ifndef V70
  80.     static    Matrix    m = {
  81.                 {1.0, 0.0, 0.0, 0.0},
  82.                 {0.0, 1.0, 0.0, 0.0},
  83.                 {0.0, 0.0, 1.0, 0.0},
  84.                 {0.0, 0.0, 0.0, 1.0}
  85.             } ;
  86.     m_copy( a, m );
  87. #else
  88.     m_copy( a, unit_matrix );
  89. #endif
  90.  
  91. }
  92.  
  93. /*    平行移動の行列生成    */
  94. Static inline void    m_mov( m, v )
  95. Matrix    m ;
  96. Vector    v ;
  97. {
  98.     m_unit( m ) ;
  99.     v_copy( m[3], v ) ;
  100. }
  101.  
  102. /*
  103.     回転の行列生成
  104.         type    0 : ｘ軸回転
  105.             1 : ｙ軸回転
  106.             2 : ｚ軸回転
  107. */
  108. Static inline void    m_rot( Matrix m, int type, Float deg )
  109. {
  110.     m_unit( m );
  111.     switch( type )
  112.     {
  113.         case 0 :
  114.             m[1][1] = cos( RAD * deg );
  115.             m[1][2] = sin( RAD * deg );
  116.             m[2][1] = - m[1][2] ;
  117.             m[2][2] = m[1][1] ;
  118.             break ;
  119.         case 1 :
  120.             m[2][2] = cos( RAD * deg );
  121.             m[2][0] = sin( RAD * deg );
  122.             m[0][2] = - m[2][0] ;
  123.             m[0][0] = m[2][2] ;
  124.             break ;
  125.         case 2 :
  126.             m[0][0] = cos( RAD * deg );
  127.             m[0][1] = sin( RAD * deg );
  128.             m[1][0] = - m[0][1] ;
  129.             m[1][1] = m[0][0] ;
  130.             break ;
  131.     }
  132. }
  133.  
  134. /*    スケールの行列生成    */
  135. Static inline void    m_scal( m, p )
  136. Matrix    m ;
  137. Point    p ;
  138. {
  139.     m_unit( m );
  140.  
  141.     m[0][0] = p[0] ;
  142.     m[1][1] = p[1] ;
  143.     m[2][2] = p[2] ;
  144. }
  145.  
  146. /*    ベクトル指定の行列生成    */
  147. Static inline void    m_vec( m, xv, zv )
  148. Matrix    m ;
  149. Vector    xv, zv ;
  150. {
  151.     Vector    yv ;
  152.  
  153.     m_unit( m ) ;
  154.  
  155.     v_pro( yv, zv, xv );
  156.     v_unit( xv, xv );
  157.     v_unit( yv, yv );
  158.     v_pro( zv, xv, yv );
  159.     v_copy( m[0], xv );
  160.     v_copy( m[1], yv );
  161.     v_copy( m[2], zv );
  162. }
  163.  
  164. /*    行列の積    */
  165. Static inline void    m_mult( ret, a1, a2 )
  166. Matrix    ret, a1, a2 ;
  167. {
  168.     int i, j, k ;
  169.     Float    w ;
  170.     Matrix    m ;
  171.  
  172.     for ( i = 0 ; i < 4 ; ++i )
  173.     {
  174.         for ( j = 0 ; j < 4 ; ++j )
  175.         {
  176.             w = 0.0 ;
  177.             for ( k = 0 ; k < 4 ; ++k )
  178.                 w += a1[i][k] * a2[k][j] ;
  179.             m[i][j] = w ;
  180.         }
  181.     }
  182.     m_copy( ret, m );
  183. }
  184.  
  185. /*    ベクトルと行列の積    */
  186. Static inline void    vec_mult_mat( ret, va, m, n, sw )
  187. Vector    *ret ;
  188. Vector    *va ;
  189. Matrix    m ;
  190. int        n, sw ;
  191. {
  192.     int     i, j;
  193.     Vector    v ;
  194.     if ( sw ) {
  195.         for ( i = 0 ; i < n ; ++i )
  196.         {
  197.             for ( j = 0 ; j < 3 ; ++j )
  198.             {
  199.                 v[j] = va[i][0]*m[0][j] + va[i][1]*m[1][j] + va[i][2]*m[2][j]
  200.                      + m[3][j];
  201.             }
  202.             v_copy( ret[i], v );
  203.         }
  204.     } else {
  205.         for ( i = 0 ; i < n ; ++i )
  206.         {
  207.             for ( j = 0 ; j < 3 ; ++j )
  208.             {
  209.                 v[j] = va[i][0]*m[0][j] + va[i][1]*m[1][j] + va[i][2]*m[2][j];
  210.             }
  211.             v_copy( ret[i], v );
  212.         }
  213.     }
  214. }
  215.  
  216. /*    転置行列    */
  217. Static inline void    m_trans( t, m )
  218. Matrix    t, m ;
  219. {
  220.     int        i, j ;
  221.     Matrix    a ;
  222.  
  223.     m_copy( a, m );
  224.  
  225.     for( i = 0 ; i < 4 ; ++i )
  226.     {
  227.         for( j = 0 ; j < 4 ; ++j )
  228.             t[i][j] = a[j][i] ;
  229.     }
  230. }
  231.  
  232. /*
  233.     逆行列
  234.     戻り値    0 : エラー
  235.         1 : 正常
  236. */
  237. #if 0
  238. static    void    m_g_mult( Matrix, Matrix, int, Float, int ) ;
  239. static    void    m_g_div( Matrix, Matrix, int, Float ) ;
  240. static    void    m_g_chg( Matrix, Matrix, int, int ) ;
  241. #endif
  242.  
  243. static inline Float m_g_value(Vector v0, Vector v1, Vector v2)
  244. {
  245.     return v0[0]*v1[1]*v2[2]+v1[0]*v2[1]*v0[2]+v2[0]*v0[1]*v1[2]
  246.           -v0[0]*v2[1]*v1[2]-v1[0]*v0[1]*v2[2]-v2[0]*v1[1]*v0[2];
  247. }
  248.  
  249. Static inline int        m_inv( inv, m )
  250. Matrix    inv, m ;
  251. {
  252.     Float val;
  253.     m_unit( inv ) ;
  254.     val = m[0][0]*m[1][1]*m[2][2]+m[1][0]*m[2][1]*m[0][2]+m[2][0]*m[0][1]*m[1][2]
  255.              -m[0][0]*m[2][1]*m[1][2]-m[1][0]*m[0][1]*m[2][2]-m[2][0]*m[1][1]*m[0][2];
  256.     if (fabs(val) < 1e-10) {
  257.         return 0;
  258.     }
  259.  
  260.     inv[0][0] =  (m[1][1]*m[2][2]-m[2][1]*m[1][2])/val;
  261.     inv[0][1] = -(m[0][1]*m[2][2]-m[2][1]*m[0][2])/val;
  262.     inv[0][2] =  (m[0][1]*m[1][2]-m[1][1]*m[0][2])/val;
  263.  
  264.     inv[1][0] = -(m[1][0]*m[2][2]-m[2][0]*m[1][2])/val;
  265.     inv[1][1] =  (m[0][0]*m[2][2]-m[2][0]*m[0][2])/val;
  266.     inv[1][2] = -(m[0][0]*m[1][2]-m[1][0]*m[0][2])/val;
  267.  
  268.     inv[2][0] =  (m[1][0]*m[2][1]-m[2][0]*m[1][1])/val;
  269.     inv[2][1] = -(m[0][0]*m[2][1]-m[2][0]*m[0][1])/val;
  270.     inv[2][2] =  (m[0][0]*m[1][1]-m[1][0]*m[0][1])/val;
  271.  
  272.     if (m[3][0] != 0.0 || m[3][1] != 0.0 || m[3][2] != 0.0) {
  273.         inv[3][0] = -m_g_value(m[1], m[2], m[3])/val;
  274.         inv[3][1] =  m_g_value(m[0], m[2], m[3])/val;
  275.         inv[3][2] = -m_g_value(m[0], m[1], m[3])/val;
  276.     }
  277.     return 1;
  278.  
  279. }
  280.  
  281.  
  282.  
  283. #if 0
  284. Static inline int        m_inv( inv, m )
  285. Matrix    inv, m ;
  286. {
  287.     int        i, j ;
  288.     Matrix    a ;
  289.  
  290.     m_copy( a, m ) ;
  291.     m_unit( inv ) ;
  292.  
  293.     for ( i = 0 ; i < 4 ; ++i )
  294.     {
  295.         if ( fabs( a[i][i] ) < 1e-10 )
  296.         {
  297.             for( j = i+1 ; j < 4 ; ++j )
  298.             {
  299.                 if ( fabs( a[j][i] ) > minscale )
  300.                     break ;
  301.             }
  302.             if ( j == 4 )
  303.                 return( 0 ) ;
  304.             m_g_chg( a, inv, i, j );
  305.         }
  306.  
  307.         m_g_div( a, inv, i, a[i][i] );
  308.         for( j = 0 ; j < 4 ; ++j )
  309.         {
  310.             if ( i == j )
  311.                 continue ;
  312.             m_g_mult( a, inv, i, a[j][i], j ) ;
  313.         }
  314.     }
  315.     return( 1 );
  316. }
  317.  
  318. static    void    m_g_mult( a1, a2, i, a, j )
  319. Matrix    a1, a2 ;
  320. int        i, j ;
  321. Float    a ;
  322. {
  323.     int k ;
  324.  
  325.     for( k = 0 ; k < 4 ; ++k )
  326.     {
  327.         a1[j][k] -= a1[i][k]*a ;
  328.         a2[j][k] -= a2[i][k]*a ;
  329.     }
  330. }
  331.  
  332. static    void    m_g_div( a1, a2, i, a )
  333. Matrix    a1, a2 ;
  334. int        i ;
  335. Float    a ;
  336. {
  337.     int k ;
  338.  
  339.     for( k = 0 ; k < 4 ; ++k )
  340.     {
  341.         a1[i][k] /= a ;
  342.         a2[i][k] /= a ;
  343.     }
  344. }
  345.  
  346. #define swap( a, b )    { Float w ; w = a ; a = b ; b = w ; }
  347.  
  348. static    void    m_g_chg( a1, a2, i, j )
  349. Matrix    a1, a2 ;
  350. int        i, j ;
  351. {
  352.     int k ;
  353.  
  354.     for( k = 0 ; k < 4 ; ++k )
  355.     {
  356.         swap( a1[i][k], a1[j][k] ) ;
  357.         swap( a2[i][k], a2[j][k] ) ;
  358.     }
  359. }
  360. #endif
  361.  
  362. #undef Static
  363.  
  364.