home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 1997 March / VPR9703A.ISO / VPR_DATA / DOGA / SOURCES / POLYEDIT.LZH / MATLIB / SETMAT.C < prev    next >
C/C++ Source or Header  |  1996-05-22  |  12KB  |  491 lines
  1. /*
  2.  *        行列演算ライブラリ
  3.  *
  4.  *        1994.6.5        Copyright T.Kobayashi
  5.  */
  6.  
  7. #include <stdio.h>
  8. #include <string.h>
  9. #include <stdlib.h>
  10. #include <assert.h>
  11.  
  12. #include "lib.h"
  13. #include "_matclas.h"
  14.  
  15. #include "inlib.h"
  16.  
  17. int        MatrixClassID ;
  18. extern    int        VectorClassID ;
  19.  
  20. static    DataStruct    UnitMatrix ;
  21. static    IntData    RotConst[3] = {
  22.     {TYPE_INT,    0,    AxisX},
  23.     {TYPE_INT,    0,    AxisY},
  24.     {TYPE_INT,    0,    AxisZ},
  25. };
  26.  
  27. static    MatrixClass    *StackAllocMatrix( DataStruct* );
  28. static    int        MatrixToString( int, int, DataStruct* );
  29. static    int        SysMatAxis( int, int, DataStruct* );
  30. static    int        SysMatrix( int, int, DataStruct* );
  31. static    int        SysMatVec( int, int, DataStruct* );
  32. static    int        SysMatMove( int, int, DataStruct* );
  33. static    int        SysMatRot( int, int, DataStruct* );
  34. static    int        SysMatScale( int, int, DataStruct* );
  35. static    int        SysMatMult( int, int, DataStruct* );
  36. static    int        SysMatInv( int, int, DataStruct* );
  37. static    int        SysMatAdd( int, int, DataStruct* );
  38. static    int        SysMatGetVector( int, int, DataStruct* );
  39. static    int        SysMatEqual( int, int, DataStruct* );
  40.  
  41.  
  42. extern    double    ToDouble( DataStruct* );
  43.  
  44. static    MatrixClass    *StackAllocMatrix( buf )
  45. DataStruct    *buf ;
  46. {
  47.     DataStruct    *top ;
  48.  
  49.     top = StackAlloc( 1 );
  50.     top->type = TYPE_OBJECT ;
  51.     top->od.ptr = ObjectDup( buf->od.ptr );
  52.     return (MatrixClass*)top->od.ptr ;
  53. }
  54.  
  55. double    ToDouble( buf )
  56. DataStruct    *buf ;
  57. {
  58.     assert( buf->type & (TYPE_INT|TYPE_REAL) );
  59.     if ( buf->type == TYPE_INT )
  60.         return (double)buf->id.i ;
  61.     else
  62.         return (double)buf->rd.r ;
  63. }
  64.  
  65. int    ToVector(int args, DataStruct *buf, Vector* vec)
  66. {
  67.     if (args <= 0) {
  68.         return 0;
  69.     } else if (args >= 3
  70.             && (buf[0].type & (TYPE_INT|TYPE_REAL))
  71.             && (buf[1].type & (TYPE_INT|TYPE_REAL))
  72.             && (buf[2].type & (TYPE_INT|TYPE_REAL))) {
  73.         vec->x = ToDouble(&buf[0]);
  74.         vec->y = ToDouble(&buf[1]);
  75.         vec->z = ToDouble(&buf[2]);
  76.         return 3;
  77.     } else if (buf->type == TYPE_OBJECT) {
  78.         if (ObjectCheck( &buf[0], VectorClassID )) {
  79.             VectorClass *v;
  80.             v = (VectorClass*)buf->od.ptr ;
  81.             vec->x = v->vec.x;
  82.             vec->y = v->vec.y;
  83.             vec->z = v->vec.z;
  84.             return 1;
  85.         }
  86.     } else if (buf->type == TYPE_ARRAY) {
  87.         DataStruct *abuf;
  88.         abuf = buf->ad.ary;
  89.         if (buf->ad.size >= 3
  90.          && (abuf[0].type & (TYPE_INT|TYPE_REAL))
  91.          && (abuf[1].type & (TYPE_INT|TYPE_REAL))
  92.          && (abuf[2].type & (TYPE_INT|TYPE_REAL))) {
  93.             vec->x = ToDouble(&abuf[0]);
  94.             vec->y = ToDouble(&abuf[1]);
  95.             vec->z = ToDouble(&abuf[2]);
  96.             return 1;
  97.         }
  98.     }
  99.     return 0;
  100. }
  101.  
  102. void    InitMatrixLibrary()
  103. {
  104.     MatrixClass    *mat ;
  105.  
  106.     MatrixClassID = NewClass( "Matrix", 0 );
  107.     NewFunction( MatrixClassID, "tostring", MatrixToString );
  108.     NewFunction( 0, "Matrix", SysMatrix );
  109.     NewFunction( MatrixClassID, "vec", SysMatVec );
  110.     NewFunction( MatrixClassID, "axis", SysMatAxis );
  111.     NewFunction( MatrixClassID, "move", SysMatMove );
  112.     NewFunction( MatrixClassID, "rot", SysMatRot );
  113.     NewFunction( MatrixClassID, "scale", SysMatScale );
  114.     NewFunction( MatrixClassID, "getvector", SysMatGetVector );
  115.     NewOperator( MatrixClassID, OPE_MULT, SysMatMult );
  116.     NewOperator( MatrixClassID, OPE_DIVIDE, SysMatInv );
  117.     NewOperator( MatrixClassID, OPE_PLUS, SysMatAdd );
  118.     NewOperator( MatrixClassID, OPE_MINUS, SysMatAdd );
  119.     NewOperator( MatrixClassID, OPE_EQ, SysMatEqual );
  120.     NewOperator( MatrixClassID, OPE_NOTEQ, SysMatEqual );
  121.  
  122.     UnitMatrix.type = TYPE_OBJECT ;
  123.     UnitMatrix.od.ptr = ObjectAlloc( sizeof( Matrix ), MatrixClassID );
  124.     mat = (MatrixClass*)UnitMatrix.od.ptr ;
  125.     MatUnit( mat->mat );
  126.     NewConst( "UNIT_MATRIX", &UnitMatrix );
  127.     NewConst( "AXISX", (DataStruct*)&RotConst[0] );
  128.     NewConst( "AXISY", (DataStruct*)&RotConst[1] );
  129.     NewConst( "AXISZ", (DataStruct*)&RotConst[2] );
  130. }
  131.  
  132. /*    表示    */
  133. static    int        MatrixToString( ident, args, buf )
  134. int        ident ;
  135. int        args ;
  136. DataStruct    *buf ;
  137. {
  138.     char    str[256] ;
  139.     MatrixClass    *m ;
  140.  
  141.     assert( buf->type == TYPE_OBJECT );
  142.     m = (MatrixClass*)buf->od.ptr ;
  143.  
  144.     sprintf( str,
  145.             " %10.4f %10.4f %10.4f\n"
  146.             " %10.4f %10.4f %10.4f\n"
  147.             " %10.4f %10.4f %10.4f\n"
  148.             " %10.4f %10.4f %10.4f\n",
  149.             m->mat[0][0], m->mat[0][1], m->mat[0][2],
  150.             m->mat[1][0], m->mat[1][1], m->mat[1][2],
  151.             m->mat[2][0], m->mat[2][1], m->mat[2][2],
  152.             m->mat[3][0], m->mat[3][1], m->mat[3][2] );
  153.  
  154.     buf = StackAlloc( 1 );
  155.     StringToObject( buf, str );
  156.     return RETURN_RETURN ;
  157. }
  158.  
  159. /*    行列の設定    */
  160. static    int        SysMatrix( ident, args, buf )
  161. int        ident ;
  162. int        args ;
  163. DataStruct    *buf ;
  164. {
  165.     int        i, j ;
  166.     MatrixClass    *m ;
  167.     DataStruct    *top ;
  168.  
  169. #if 0
  170.     ArgCheck( "axis", args, buf,
  171.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL,
  172.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL,
  173.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL,
  174.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_NOASN );
  175.  
  176.  
  177.     top = StackAlloc( 1 );
  178.     top->type = TYPE_OBJECT ;
  179.     top->od.ptr = ObjectAlloc( sizeof( Matrix ), MatrixClassID );
  180.     m = (MatrixClass*)top->od.ptr ;
  181.  
  182.     for( i = 0 ; i < 4 ; i++ )
  183.     {
  184.         for( j = 0 ; j < 3 ; j++ )
  185.             m->mat[i][j] = ToDouble( &buf[i*3+j] );
  186.     }
  187. #endif
  188.  
  189.     Vector v[4];
  190.     int i0, i1, i2, i3;
  191.     if ((i0 = ToVector(args,          buf,          &v[0])) == 0
  192.      || (i1 = ToVector(args-i0,       buf+i0,       &v[1])) == 0
  193.      || (i2 = ToVector(args-i0-i1,    buf+i0+i1,    &v[2])) == 0) {
  194.         ExecError( "Matrix: 引数の型が不正です。" );
  195.     }
  196.     if ((i3 = ToVector(args-i0-i1-i2, buf+i0+i1+i2, &v[3])) == 0) {
  197.         v[3].x = v[3].y = v[3].z = 0.0;
  198.     }
  199.     top = StackAlloc( 1 );
  200.     top->type = TYPE_OBJECT ;
  201.     top->od.ptr = ObjectAlloc( sizeof( Matrix ), MatrixClassID );
  202.     m = (MatrixClass*)top->od.ptr ;
  203.  
  204.     for( i = 0 ; i < 4 ; i++ )
  205.     {
  206.         m->mat[i][0] = v[i].x;
  207.         m->mat[i][1] = v[i].y;
  208.         m->mat[i][2] = v[i].z;
  209.     }
  210.  
  211.     return RETURN_RETURN ;
  212. }
  213.  
  214.  
  215. /*    軸変換の行列生成    */
  216. static    int        SysMatAxis( ident, args, buf )
  217. int        ident ;
  218. int        args ;
  219. DataStruct    *buf ;
  220. {
  221.     MatrixClass    *m ;
  222.  
  223.     ArgCheck( "axis", args, buf, TYPE_OBJECT, TYPE_INT, TYPE_INT, TYPE_NOASN );
  224.  
  225.     m = StackAllocMatrix( &buf[0] );
  226.     MatAxis( m->mat, buf[1].id.i, buf[2].id.i );
  227.  
  228.     return RETURN_RETURN ;
  229. }
  230.  
  231. /*    ベクトル指定の行列生成    */
  232. static    int        SysMatVec( ident, args, buf )
  233. int        ident ;
  234. int        args ;
  235. DataStruct    *buf ;
  236. {
  237.     MatrixClass    *m ;
  238. #if 0
  239.     VectorClass    *vec1, *vec2 ;
  240.     ArgCheck( "axis", args, buf, TYPE_OBJECT,
  241.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL,
  242.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL,
  243.         TYPE_NOASN );
  244.  
  245.     m = StackAllocMatrix( &buf[0] );
  246.     vec1 = (VectorClass*)buf[1].od.ptr ;
  247.     vec2 = (VectorClass*)buf[2].od.ptr ;
  248.     MatVec( m->mat,
  249.         ToDouble( &buf[1] ), ToDouble( &buf[2] ), ToDouble( &buf[3] ),
  250.         ToDouble( &buf[4] ), ToDouble( &buf[5] ), ToDouble( &buf[6] ) );
  251. #endif
  252.     Vector v0, v1;
  253.     int i0, i1;
  254.     if ((i0 = ToVector(args,   buf+1,   &v0)) == 0
  255.      || (i1 = ToVector(args-i0,buf+1+i0,&v1)) == 0) {
  256.         ExecError( "vec: 引数の型が不正です。" );
  257.     }
  258.     m = StackAllocMatrix( &buf[0] );
  259.     MatVec(m->mat, v0.x, v0.y, v0.z, v1.x, v1.y, v1.z);
  260.  
  261.     return RETURN_RETURN ;
  262. }
  263.  
  264. /*    平行移動の行列合成    */
  265. static    int        SysMatMove( ident, args, buf )
  266. int        ident ;
  267. int        args ;
  268. DataStruct    *buf ;
  269. {
  270.     MatrixClass    *m ;
  271. #if 0
  272.     ArgCheck( "move", args, buf, TYPE_OBJECT,
  273.             TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_NOASN );
  274.  
  275.     m = StackAllocMatrix( &buf[0] );
  276.     MatMove( m->mat, ToDouble( &buf[1] ), ToDouble( &buf[2] ), ToDouble( &buf[3] ) );
  277. #endif
  278.     Vector v;
  279.     if (ToVector(args, buf+1, &v) == 0) {
  280.         ExecError( "move: 引数の型が不正です。" );
  281.     }
  282.     m = StackAllocMatrix( &buf[0] );
  283.     MatMove(m->mat, v.x, v.y, v.z);
  284.     return RETURN_RETURN ;
  285. }
  286.  
  287. /*    回転の行列合成    */
  288. static    int        SysMatRot( ident, args, buf )
  289. int        ident ;
  290. int        args ;
  291. DataStruct    *buf ;
  292. {
  293.     MatrixClass    *m ;
  294.  
  295.     ArgCheck( "rot", args, buf, TYPE_OBJECT, TYPE_INT, TYPE_INT|TYPE_REAL, TYPE_NOASN );
  296.     m = StackAllocMatrix( &buf[0] );
  297.     MatRot( m->mat, buf[1].id.i, ToDouble( &buf[2] ) );
  298.  
  299.     return RETURN_RETURN ;
  300. }
  301.  
  302. /*    スケールの行列生成    */
  303. static    int        SysMatScale( ident, args, buf )
  304. int        ident ;
  305. int        args ;
  306. DataStruct    *buf ;
  307. {
  308.     MatrixClass    *m ;
  309. #if 0
  310.     ArgCheck( "scale", args, buf, TYPE_OBJECT,
  311.             TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_NOASN );
  312.     m = StackAllocMatrix( &buf[0] );
  313.     MatScale( m->mat, ToDouble( &buf[1] ), ToDouble( &buf[2] ), ToDouble( &buf[3] ) );
  314. #endif
  315.     Vector v;
  316.     if (ToVector(args, buf+1, &v) == 0) {
  317.         ExecError( "move: 引数の型が不正です。" );
  318.     }
  319.     m = StackAllocMatrix( &buf[0] );
  320.     MatScale(m->mat, v.x, v.y, v.z);
  321.     return RETURN_RETURN ;
  322. }
  323.  
  324. /*    スケールの行列生成    */
  325. static    int        SysMatGetVector( ident, args, buf )
  326. int        ident ;
  327. int        args ;
  328. DataStruct    *buf ;
  329. {
  330.     MatrixClass    *m ;
  331.     VectorClass *v ;
  332.     int n;
  333.     m = (MatrixClass*)buf[0].od.ptr;
  334.     if (buf[1].type == TYPE_ARRAY) {
  335.         ArgCheck( "getvector", args, buf, TYPE_OBJECT, TYPE_ARRAY, TYPE_NOASN );
  336.         if (buf[1].ad.size < 4) {
  337.             ExecError("引数の配列の要素数が足りません。(getvector)");
  338.         }
  339.         buf = buf[1].ad.ary;
  340.         for (n = 0; n < 4; ++n) {
  341.             v = (VectorClass*)ObjectAlloc( sizeof( VectorClass ), VectorClassID );
  342.             v->vec.x = m->mat[n][0];
  343.             v->vec.y = m->mat[n][1];
  344.             v->vec.z = m->mat[n][2];
  345.             buf[n].type = TYPE_OBJECT;
  346.             buf[n].od.ptr = (Object*)v;
  347.         }
  348.         return RETURN_VOID ;
  349.     } else {
  350.         ArgCheck( "getvector", args, buf, TYPE_OBJECT, TYPE_INT, TYPE_NOASN );
  351.         n = buf[1].id.i;
  352.         if (n < 0 || n >= 4) {
  353.             StackPushBoolean(FALSE);
  354.         } else {
  355.             v = (VectorClass*)ObjectAlloc( sizeof( VectorClass ), VectorClassID );
  356.             v->vec.x = m->mat[n][0];
  357.             v->vec.y = m->mat[n][1];
  358.             v->vec.z = m->mat[n][2];
  359.             buf = StackAlloc( 1 );
  360.             buf->type = TYPE_OBJECT ;
  361.             buf->od.ptr = (Object*)v ;
  362.         }
  363.     }
  364.     return RETURN_RETURN ;
  365. }
  366.  
  367.  
  368. /*    行列の積    */
  369. static    int        SysMatMult( ident, args, buf )
  370. int        ident ;
  371. int        args ;
  372. DataStruct    *buf ;
  373. {
  374.     double    a ;
  375.     MatrixClass    *m1, *m2, *m ;
  376.  
  377.     assert( buf[0].type == TYPE_OBJECT );
  378.     m = StackAllocMatrix( &buf[0] );
  379.     if ( ObjectCheck( &buf[1], MatrixClassID ) )
  380.     {
  381.         m1 = (MatrixClass*)buf[0].od.ptr ;
  382.         m2 = (MatrixClass*)buf[1].od.ptr ;
  383.         MatMult( m->mat, m1->mat, m2->mat );
  384.     }
  385.     else if ( buf[1].type & (TYPE_INT|TYPE_REAL) )
  386.     {
  387.         a = ToDouble( &buf[1] );
  388.         MatScale( m->mat, a, a, a );
  389.     }
  390.     else
  391.     {
  392.         ExecError( "型が不正です。（演算子*）" );
  393.     }
  394.     return RETURN_RETURN ;
  395. }
  396.  
  397. /*    逆行列    */
  398. static    int        SysMatInv( ident, args, buf )
  399. int        ident ;
  400. int        args ;
  401. DataStruct    *buf ;
  402. {
  403.     MatrixClass    *m1, *m2 ;
  404.     Matrix    mat ;
  405.  
  406.     assert( buf[0].type == TYPE_OBJECT );
  407.     m1 = StackAllocMatrix( &buf[0] );
  408.  
  409.     if ( ObjectCheck( &buf[1], MatrixClassID ) )
  410.     {
  411.         m2 = (MatrixClass*)buf[1].od.ptr ;
  412.         MatCopy( mat, m2->mat );
  413.         MatInv( mat );
  414.         MatMult( m1->mat, m1->mat, mat );
  415.     }
  416.     else
  417.     {
  418.         ExecError( "型が不正です。（演算子/）" );
  419.     }
  420.     return RETURN_RETURN ;
  421. }
  422.  
  423. static    int        SysMatAdd( ident, args, buf )
  424. int        ident ;
  425. int        args ;
  426. DataStruct    *buf ;
  427. {
  428.     MatrixClass    *m1, *m2 ;
  429.  
  430.     assert( buf[0].type == TYPE_OBJECT );
  431.  
  432.     if ( ObjectCheck( &buf[1], MatrixClassID ) )
  433.     {
  434.         m1 = StackAllocMatrix( &buf[0] );
  435.         m2 = (MatrixClass*)buf[1].od.ptr ;
  436.         switch( ident )
  437.         {
  438.             case OPE_PLUS:
  439.                 MatAdd( m1->mat, m1->mat, m2->mat );
  440.                 break ;
  441.             case OPE_MINUS:
  442.                 MatSub( m1->mat, m1->mat, m2->mat );
  443.                 break ;
  444.             default:
  445.                 assert( FALSE );
  446.         }
  447.     }
  448.     return RETURN_RETURN ;
  449. }
  450.  
  451. static    int        SysMatEqual( ident, args, buf )
  452. int        ident ;
  453. int        args ;
  454. DataStruct    *buf ;
  455. {
  456.     MatrixClass    *m1, *m2 ;
  457.     int flag;
  458.     int i, j;
  459.  
  460.     assert( buf[0].type == TYPE_OBJECT );
  461.  
  462.     if ( ObjectCheck( &buf[1], MatrixClassID ) )
  463.     {
  464.         m1 = (MatrixClass*)buf[0].od.ptr ;
  465.         m2 = (MatrixClass*)buf[1].od.ptr ;
  466.  
  467.         flag = TRUE;
  468.         for( i = 0 ; i < 4 ; i++ )
  469.         {
  470.             for( j = 0 ; j < 3 ; j++ ) {
  471.                 flag &= (m1->mat[i][j] == m2->mat[i][j]);
  472.             }
  473.         }
  474.  
  475.         switch( ident )
  476.         {
  477.             case OPE_EQ:
  478.                 StackPushBoolean(flag);
  479.                 break ;
  480.             case OPE_NOTEQ:
  481.                 StackPushBoolean(!flag);
  482.                 break ;
  483.             default:
  484.                 assert( FALSE );
  485.         }
  486.         return RETURN_RETURN;
  487.     }
  488.     return RETURN_VOID ;
  489. }
  490.  
  491.