home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 1997 March / VPR9703A.ISO / VPR_DATA / DOGA / SOURCES / POLYEDIT.LZH / MATLIB / SETVEC.C < prev    next >
C/C++ Source or Header  |  1996-05-22  |  9KB  |  376 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 <math.h>
  11. #include <assert.h>
  12.  
  13. #include "lib.h"
  14. #include "_matclas.h"
  15.  
  16. #include "inlib.h"
  17.  
  18. extern    int        MatrixClassID ;
  19. int        VectorClassID ;
  20.  
  21. extern    double    ToDouble( DataStruct* );
  22.  
  23. static    int        VectorToString( int, int, DataStruct* );
  24. static    int        SysVector( int, int, DataStruct* );
  25. static    int        SysVecUnit( int, int, DataStruct* );
  26. static    int        SysVecX( int, int, DataStruct* );
  27. static    int        SysVecY( int, int, DataStruct* );
  28. static    int        SysVecZ( int, int, DataStruct* );
  29. static    int        SysVecAbs( int, int, DataStruct* );
  30. static    int        SysVecElement( int, int, DataStruct* );
  31. static    int        SysScalPro( int, int, DataStruct* );
  32. static    int        SysVecPro( int, int, DataStruct* );
  33. static    int        SysVecAdd( int, int, DataStruct* );
  34. static    int        SysVecMultMat( int, int, DataStruct* );
  35. static    int        SysMinus( int, int, DataStruct* );
  36. static    int        SysVecEqual( int, int, DataStruct* );
  37.  
  38. VectorClass    *StackAllocVector( buf )
  39. DataStruct    *buf ;
  40. {
  41.     DataStruct    *top ;
  42.  
  43.     top = StackAlloc( 1 );
  44.     top->type = TYPE_OBJECT ;
  45.     top->od.ptr = ObjectDup( buf->od.ptr );
  46.     return (VectorClass*)top->od.ptr ;
  47. }
  48.  
  49. void    InitVectorLibrary()
  50. {
  51.     VectorClassID = NewClass( "Vector", 0 );
  52.     NewFunction( 0, "vector", SysVector );
  53.     NewFunction( VectorClassID, "tostring", VectorToString );
  54.     NewFunction( VectorClassID, "unit", SysVecUnit );
  55.     NewFunction( VectorClassID, "vx", SysVecX );
  56.     NewFunction( VectorClassID, "vy", SysVecY );
  57.     NewFunction( VectorClassID, "vz", SysVecZ );
  58.     NewFunction( VectorClassID, "element", SysVecElement );
  59.     NewFunction( VectorClassID, "abs", SysVecAbs );
  60.     NewOperator( VectorClassID, OPE_DOT, SysScalPro );
  61.     NewOperator( VectorClassID, OPE_MULT, SysVecPro );
  62.     NewOperator( VectorClassID, OPE_PLUS, SysVecAdd );
  63.     NewOperator( VectorClassID, OPE_MINUS, SysVecAdd );
  64.     NewOperator( VectorClassID, OPE_MINUS1, SysMinus );
  65.     NewOperator( VectorClassID, OPE_EQ, SysVecEqual);
  66.     NewOperator( VectorClassID, OPE_NOTEQ, SysVecEqual);
  67. }
  68.  
  69. /*    表示    */
  70. static    int        VectorToString( ident, args, buf )
  71. int        ident ;
  72. int        args ;
  73. DataStruct    *buf ;
  74. {
  75.     char    str[256] ;
  76.     VectorClass    *v ;
  77.  
  78.     assert( buf->type == TYPE_OBJECT );
  79.     v = (VectorClass*)buf->od.ptr ;
  80.     sprintf( str, "( %8.4f, %8.4f, %8.4f )", v->vec.x, v->vec.y, v->vec.z );
  81.  
  82.     buf = StackAlloc( 1 );
  83.     StringToObject( buf, str );
  84.     return RETURN_RETURN ;
  85. }
  86.  
  87. /*    ベクトルを得る    */
  88. static    int        SysVector( ident, args, buf )
  89. int        ident ;
  90. int        args ;
  91. DataStruct    *buf ;
  92. {
  93.     extern int    ToVector(int args, DataStruct *buf, Vector* vec);
  94.     VectorClass    *vec ;
  95.     Vector v;
  96. #if 0
  97.     ArgCheck( "vector", args, buf,
  98.         TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_INT|TYPE_REAL, TYPE_NOASN );
  99.     vec = (VectorClass*)ObjectAlloc( sizeof( VectorClass ), VectorClassID );
  100.  
  101.     vec->vec.x = ToDouble( &buf[0] );
  102.     vec->vec.y = ToDouble( &buf[1] );
  103.     vec->vec.z = ToDouble( &buf[2] );
  104. #endif
  105.     if (ToVector(args, buf, &v) == 0) {
  106.         ExecError( "引数の型が不正です。(Vector:vector)" );
  107.     }
  108.     vec = (VectorClass*)ObjectAlloc( sizeof( VectorClass ), VectorClassID );
  109.     vec->vec = v;
  110.     buf = StackAlloc( 1 );
  111.     buf->type = TYPE_OBJECT ;
  112.     buf->od.ptr = (Object*)vec ;
  113.  
  114.     return RETURN_RETURN ;
  115. }
  116.  
  117. /*    正規化    */
  118. static    int        SysVecUnit( ident, args, buf )
  119. int        ident ;
  120. int        args ;
  121. DataStruct    *buf ;
  122. {
  123.     VectorClass    *v ;
  124.  
  125.     ArgCheck( "unit", args, buf, TYPE_OBJECT, TYPE_NOASN );
  126.  
  127.     v = StackAllocVector( &buf[0] );
  128.     VecUnit( &v->vec );
  129.  
  130.     return RETURN_RETURN ;
  131. }
  132.  
  133. /*    成分を返す    */
  134. static    int        SysVecX( ident, args, buf )
  135. int        ident ;
  136. int        args ;
  137. DataStruct    *buf ;
  138. {
  139.     VectorClass    *v ;
  140.  
  141.     assert( buf[0].type == TYPE_OBJECT );
  142.     v = (VectorClass*)buf[0].od.ptr ;
  143.     StackPushReal( v->vec.x );
  144.     return RETURN_RETURN ;
  145. }
  146. static    int        SysVecY( ident, args, buf )
  147. int        ident ;
  148. int        args ;
  149. DataStruct    *buf ;
  150. {
  151.     VectorClass    *v ;
  152.  
  153.     assert( buf[0].type == TYPE_OBJECT );
  154.     v = (VectorClass*)buf[0].od.ptr ;
  155.     StackPushReal( v->vec.y );
  156.     return RETURN_RETURN ;
  157. }
  158. static    int        SysVecZ( ident, args, buf )
  159. int        ident ;
  160. int        args ;
  161. DataStruct    *buf ;
  162. {
  163.     VectorClass    *v ;
  164.  
  165.     assert( buf[0].type == TYPE_OBJECT );
  166.     v = (VectorClass*)buf[0].od.ptr ;
  167.     StackPushReal( v->vec.z );
  168.     return RETURN_RETURN ;
  169. }
  170.  
  171.  
  172. /*    各要素を得る    */
  173. static    int        SysVecElement( ident, args, buf )
  174. int        ident ;
  175. int        args ;
  176. DataStruct    *buf ;
  177. {
  178.     VectorClass    *v ;
  179.     ArgCheck( "Vector", args, buf, TYPE_OBJECT, TYPE_ARRAY, TYPE_NOASN );
  180.  
  181.     if ( buf[1].ad.size < 3 )
  182.         ExecError( "引数配列のサイズが不正です。(Vector:element)" );
  183.  
  184.     v = (VectorClass*)buf[0].od.ptr ;
  185.  
  186.     buf = buf[1].ad.ary ;
  187.     buf[0].type = buf[1].type = buf[2].type = TYPE_REAL ;
  188.     buf[0].rd.r = v->vec.x ;
  189.     buf[1].rd.r = v->vec.y ;
  190.     buf[2].rd.r = v->vec.z ;
  191.  
  192.     return RETURN_VOID ;
  193.  
  194.     StackPushReal( VecAbs( &v->vec ) );
  195.     return RETURN_RETURN ;
  196. }
  197.  
  198. /*    長さを得る    */
  199. static    int        SysVecAbs( ident, args, buf )
  200. int        ident ;
  201. int        args ;
  202. DataStruct    *buf ;
  203. {
  204.     VectorClass    *v ;
  205.  
  206.     assert( buf[0].type == TYPE_OBJECT );
  207.     v = (VectorClass*)buf[0].od.ptr ;
  208.     StackPushReal( VecAbs( &v->vec ) );
  209.     return RETURN_RETURN ;
  210. }
  211.  
  212. /*    スカラー積    */
  213. static    int        SysScalPro( ident, args, buf )
  214. int        ident ;
  215. int        args ;
  216. DataStruct    *buf ;
  217. {
  218.     VectorClass    *v1, *v2 ;
  219.  
  220.     assert( buf[0].type == TYPE_OBJECT );
  221.     v1 = (VectorClass*)buf[0].od.ptr ;
  222.     if ( ObjectCheck( &buf[1], VectorClassID ) )
  223.     {
  224.         v2 = (VectorClass*)buf[1].od.ptr ;
  225.         StackPushReal( ScalPro( &v1->vec, &v2->vec ) );
  226.     }
  227.     else if ( ObjectCheck( &buf[1], MatrixClassID ) )
  228.     {
  229.         SysVecMultMat( ident, args, buf );
  230.     }
  231.     else
  232.     {
  233.         ExecError( "型が不正です。（演算子.）" );
  234.     }
  235.     return RETURN_RETURN ;
  236. }
  237.  
  238. /*    ベクトル積    */
  239. static    int        SysVecPro( ident, args, buf )
  240. int        ident ;
  241. int        args ;
  242. DataStruct    *buf ;
  243. {
  244.     VectorClass    *ret, *v1, *v2 ;
  245.  
  246.     assert( buf[0].type == TYPE_OBJECT );
  247.  
  248.     if ( ObjectCheck( &buf[1], VectorClassID ) )
  249.     {
  250.         ret = (VectorClass*)ObjectAlloc( sizeof( VectorClass ), VectorClassID );
  251.         v1 = (VectorClass*)buf[0].od.ptr ;
  252.         v2 = (VectorClass*)buf[1].od.ptr ;
  253.         VecPro( &ret->vec, &v1->vec, &v2->vec );
  254.         buf = StackAlloc( 1 );
  255.         buf->type = TYPE_OBJECT ;
  256.         buf->od.ptr = (Object*)ret ;
  257.     }
  258.     else if ( ObjectCheck( &buf[1], MatrixClassID ) )
  259.     {
  260.         SysVecMultMat( ident, args, buf );
  261.     }
  262.     else if ( buf[1].type & (TYPE_INT|TYPE_REAL) )
  263.     {
  264.         v1 = StackAllocVector( &buf[0] );
  265.         VecMult( &v1->vec, &v1->vec, ToDouble( &buf[1] ) );
  266.     }
  267.     else
  268.     {
  269.         ExecError( "型が不正です。（演算子*）" );
  270.     }
  271.     return RETURN_RETURN ;
  272. }
  273.  
  274. static    int        SysVecAdd( ident, args, buf )
  275. int        ident ;
  276. int        args ;
  277. DataStruct    *buf ;
  278. {
  279.     VectorClass    *v1, *v2 ;
  280.  
  281.     assert( buf[0].type == TYPE_OBJECT );
  282.  
  283.     if ( ObjectCheck( &buf[1], VectorClassID ) )
  284.     {
  285.         v1 = StackAllocVector( &buf[0] );
  286.         v2 = (VectorClass*)buf[1].od.ptr ;
  287.         switch( ident )
  288.         {
  289.             case OPE_PLUS:
  290.                 VecAdd( &v1->vec, &v1->vec, &v2->vec );
  291.                 break ;
  292.             case OPE_MINUS:
  293.                 VecSub( &v1->vec, &v1->vec, &v2->vec );
  294.                 break ;
  295.             default:
  296.                 assert( FALSE );
  297.         }
  298.     }
  299.     return RETURN_RETURN ;
  300. }
  301.  
  302. static    int        SysVecMultMat( ident, args, buf )
  303. int        ident ;
  304. int        args ;
  305. DataStruct    *buf ;
  306. {
  307.     int            i ;
  308.     VectorClass    *v ;
  309.     MatrixClass    *m ;
  310.     float        r[3] ;
  311.  
  312.     m = (MatrixClass*)buf[1].od.ptr ;
  313.     v = StackAllocVector( &buf[0] );
  314.     for( i = 0 ; i < 3 ; i++ )
  315.     {
  316.         r[i] =    v->vec.x * m->mat[0][i] +
  317.                 v->vec.y * m->mat[1][i] +
  318.                 v->vec.z * m->mat[2][i] +
  319.                 m->mat[3][i] ;
  320.     }
  321.     v->vec.x = r[0] ;
  322.     v->vec.y = r[1] ;
  323.     v->vec.z = r[2] ;
  324.  
  325.     return RETURN_RETURN ;
  326. }
  327.  
  328. static    int        SysMinus( ident, args, buf )
  329. int        ident ;
  330. int        args ;
  331. DataStruct    *buf ;
  332. {
  333.     VectorClass    *v;
  334.  
  335.     v = StackAllocVector( &buf[0] );
  336.     v->vec.x = -v->vec.x;
  337.     v->vec.y = -v->vec.y;
  338.     v->vec.z = -v->vec.z;
  339.  
  340.     return RETURN_RETURN ;
  341. }
  342.  
  343. static    int        SysVecEqual( ident, args, buf )
  344. int        ident ;
  345. int        args ;
  346. DataStruct    *buf ;
  347. {
  348.     VectorClass    *v1, *v2 ;
  349.     int flag;
  350.  
  351.     assert( buf[0].type == TYPE_OBJECT );
  352.  
  353.     if ( ObjectCheck( &buf[1], VectorClassID ) )
  354.     {
  355.         v1 = (VectorClass*)buf[0].od.ptr;
  356.         v2 = (VectorClass*)buf[1].od.ptr ;
  357.         flag = (v1->vec.x == v2->vec.x)
  358.             && (v1->vec.y == v2->vec.y)
  359.             && (v1->vec.z == v2->vec.z);
  360.         switch( ident )
  361.         {
  362.             case OPE_EQ:
  363.                 StackPushBoolean( flag );
  364.                 break ;
  365.             case OPE_NOTEQ:
  366.                 StackPushBoolean( !flag );
  367.                 break ;
  368.             default:
  369.                 assert( FALSE );
  370.         }
  371.         return RETURN_RETURN;
  372.     }
  373.     return RETURN_VOID;
  374. }
  375.  
  376.