home *** CD-ROM | disk | FTP | other *** search
/ APDL Public Domain 2 / APDL_PD2A.iso / demos / demo_1 / demo4 / !Demo4 / c / sol < prev    next >
Encoding:
Text File  |  1992-02-10  |  11.3 KB  |  410 lines
  1. /******************************************************************************
  2. *                                                                             *
  3. *    sol.c                                                                    *
  4. *                                                                             *
  5. ******************************************************************************/
  6.  
  7. #include <stdlib.h>
  8. #include <math.h>
  9. #include "GrpLib.h"
  10. #include "extern.h"
  11.  
  12. /*****************************************************************************/
  13.  
  14. void ProdVectorielNI(vec4 a,double b[],double c[])
  15. {
  16.   double n,d[4];
  17.  
  18.   d[0] = -(b[1]*c[2] - b[2]*c[1]);
  19.   d[1] = -(b[2]*c[0] - b[0]*c[2]);
  20.   d[2] = -(b[0]*c[1] - b[1]*c[0]);
  21.  
  22.   n    = sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);
  23.   a[0] = (int)(16384*d[0]/n);
  24.   a[1] = (int)(16384*d[1]/n);
  25.   a[2] = (int)(16384*d[2]/n);
  26. }
  27.  
  28. /*****************************************************************************/
  29.  
  30. void TransformPt(vec3 q,mat33 m,vec3 t,vec3 p)
  31. {
  32.   q[0] = ((m[0][0]*p[0] + m[1][0]*p[1] + m[2][0]*p[2])>>14) + t[0];
  33.   q[1] = ((m[0][1]*p[0] + m[1][1]*p[1] + m[2][1]*p[2])>>14) + t[1];
  34.   q[2] = ((m[0][2]*p[0] + m[1][2]*p[1] + m[2][2]*p[2])>>14) + t[2];
  35. }
  36.  
  37. /*****************************************************************************/
  38.  
  39. void trace(vec4 pts[],int i,int k,int i2,int k2,int tgx,
  40.            char *sprite,int pgx,int pgz)
  41. {
  42.   GrpReg[ 0] = pts[2*(i+   (k   )*tgx)+1][0];
  43.   GrpReg[ 1] = pts[2*(i+   (k   )*tgx)+1][1];
  44.   GrpReg[ 2] = pts[2*(i+i2+(k+k2)*tgx)+1][0];
  45.   GrpReg[ 3] = pts[2*(i+i2+(k+k2)*tgx)+1][1];
  46.   GrpReg[ 4] = pts[2*(i+1 +(k+1 )*tgx)+1][0];
  47.   GrpReg[ 5] = pts[2*(i+1 +(k+1 )*tgx)+1][1];
  48.   GrpReg[ 6] =  i    *pgx;
  49.   GrpReg[ 7] =  k    *pgz;
  50.   GrpReg[ 8] = (i+i2)*pgx;
  51.   GrpReg[ 9] = (k+k2)*pgz;
  52.   GrpReg[10] = (i+1) *pgx;
  53.   GrpReg[11] = (k+1) *pgz;
  54.   GrpReg[12] = (int)sprite;
  55.   CallGrp(GrpReg,GrpStack,Spr2DPoly3);
  56. }
  57.  
  58. /*****************************************************************************/
  59.  
  60. void InitSol(Sol *sold,char *spr,int *alt,int scalex,int scalez)
  61. {
  62.   int  tgx,tgz,pgx,pgz;
  63.   int  nbpts,nbnms,i,k;
  64.   vec4 *normals,*points;
  65.   int  *altd;
  66.   double b[4],c[4];
  67.  
  68.   sold->sprite    = spr;
  69.   sold->altitudes = alt;
  70.  
  71.   tgx = (sold->tgx    = alt[ 7]);
  72.   tgz = (sold->tgz    = alt[ 8]);
  73.   pgx = (sold->pgx    = alt[ 9]);
  74.   pgz = (sold->pgz    = alt[10]);
  75.          sold->scalex = scalex;
  76.          sold->scalez = scalez;
  77.   altd = alt + (alt[6]>>2);
  78.  
  79.   nbpts   = tgx*tgz;
  80.   points  = ( sold->points  = malloc(    (nbpts+1) * sizeof(vec4)) );
  81.               sold->pscreen = malloc(2 * (nbpts+1) * sizeof(vec4));
  82.   points[nbpts][3]  = -1;
  83.   sold->pscreen[2*nbpts][3] = -1;
  84.  
  85.   for(k=0 ; k<tgz ; k++)
  86.     for(i=0 ; i<tgx ; i++) {
  87.       int alt1;
  88.       alt1 = altd[i+k*tgx];
  89.       if (i==0    ) { alt1=0; }
  90.       if (i==tgx-1) { alt1=0; }
  91.       if (k==0    ) { alt1=0; }
  92.       if (k==tgz-1) { alt1=0; }
  93.       points[i+k*tgx][0] = scalex*i*pgx;
  94.       points[i+k*tgx][1] = alt1;
  95.       points[i+k*tgx][2] = scalez*k*pgz;
  96.       points[i+k*tgx][3] = 0;
  97.     }
  98.  
  99.   nbnms   = 2*(tgx-1)*(tgz-1);
  100.   normals = ( sold->normals = malloc((nbnms+1) * sizeof(vec4)) );
  101.               sold->nscreen = malloc((nbnms+1) * sizeof(vec4));
  102.   normals[nbnms][3] = -1;
  103.  
  104.   for(k=0 ; k<tgz-1 ; k++)
  105.     for(i=0 ; i<tgx-1 ; i++) {
  106.       int alt1,alt2,alt3,alt4;
  107.       alt1 = altd[(i  )+(k  )*tgx];
  108.       alt2 = altd[(i+1)+(k  )*tgx];
  109.       alt3 = altd[(i  )+(k+1)*tgx];
  110.       alt4 = altd[(i+1)+(k+1)*tgx];
  111.       if (i==0    ) { alt1=0; alt3=0; }
  112.       if (i==tgx-2) { alt2=0; alt4=0;}
  113.       if (k==0    ) { alt1=0; alt2=0;}
  114.       if (k==tgz-2) { alt3=0; alt4=0;}
  115.       b[0] = scalex*pgx;
  116.       b[1] = alt2 - alt1;
  117.       b[2] = 0;
  118.       c[0] = 0;
  119.       c[1] = alt4 - alt2;
  120.       c[2] = scalez*pgz;
  121.       ProdVectorielNI(normals[2*(i+k*(tgx-1))  ],b,c);
  122.       b[0] = scalex*pgx;
  123.       b[1] = alt4 - alt3;
  124.       b[2] = 0;
  125.       c[0] = 0;
  126.       c[1] = alt3 - alt1;
  127.       c[2] = scalez*pgz;
  128.       ProdVectorielNI(normals[2*(i+k*(tgx-1))+1],b,c);
  129.     }
  130. }
  131.  
  132. /*****************************************************************************/
  133.  
  134. void PlotSol(Envi *envi,Object *obs,Object *sol,Sol *sold)
  135. {
  136.   vec4  Obj4Rep[4];
  137.   mat33 inv;
  138.   vec3  transl,p;
  139.   int   vu;
  140.  
  141.   vec4  pex[5],pexs[10];
  142.   int   zneg=0,code,codeand=0;
  143.  
  144.   vec4  *pscreen,*nscreen;
  145.  
  146.   int   tgx,tgz,pgx,pgz;
  147.   char  *sp;
  148.   int   x,z,nx,nz;
  149.   int   i,k;
  150.  
  151.   tgx = sold->tgx;
  152.   tgz = sold->tgz;
  153.   pgx = sold->pgx;
  154.   pgz = sold->pgz;
  155.   sp  = sold->sprite;
  156.  
  157.   InvMat(inv,obs->Rep);
  158.   MkVec3(transl,sol->Org[0] - obs->Org[0],
  159.                 sol->Org[1] - obs->Org[1],
  160.                 sol->Org[2] - obs->Org[2]);
  161.  
  162.   GrpReg[0] = (int)inv;
  163.   GrpReg[1] = (int)Obj4Rep;
  164.   GrpReg[2] = (int)sol->Rep;
  165.   GrpReg[3] = (int)transl;
  166.   CallGrp(GrpReg,GrpStack,RepToRepIn4Rep); /*Obj4Rep=inv*(transl o SolRep)*/
  167.  
  168. /* verifie si le sol est dans la piramide de vision */
  169.  
  170.   pex[0][0] = sold->points[                0][0];
  171.   pex[0][1] = sold->points[                0][1];
  172.   pex[0][2] = sold->points[                0][2];
  173.   pex[0][3] = 0;
  174.   pex[1][0] = sold->points[            tgx-1][0];
  175.   pex[1][1] = sold->points[            tgx-1][1];
  176.   pex[1][2] = sold->points[            tgx-1][2];
  177.   pex[1][3] = 0;
  178.   pex[2][0] = sold->points[(tgz-1)*tgx      ][0];
  179.   pex[2][1] = sold->points[(tgz-1)*tgx      ][1];
  180.   pex[2][2] = sold->points[(tgz-1)*tgx      ][2];
  181.   pex[2][3] = 0;
  182.   pex[3][0] = sold->points[(tgz-1)*tgx+tgx-1][0];
  183.   pex[3][1] = sold->points[(tgz-1)*tgx+tgx-1][1];
  184.   pex[3][2] = sold->points[(tgz-1)*tgx+tgx-1][2];
  185.   pex[3][3] = 0;
  186.   pex[4][3] = -1;
  187.  
  188.   pexs[8][3] = -1;
  189.  
  190.   GrpReg[ 0] = Obj4Rep[0][0];
  191.   GrpReg[ 1] = Obj4Rep[0][1];
  192.   GrpReg[ 2] = Obj4Rep[0][2];
  193.   GrpReg[ 3] = Obj4Rep[1][0];
  194.   GrpReg[ 4] = Obj4Rep[1][1];
  195.   GrpReg[ 5] = Obj4Rep[1][2];
  196.   GrpReg[ 6] = Obj4Rep[2][0];
  197.   GrpReg[ 7] = Obj4Rep[2][1];
  198.   GrpReg[ 8] = Obj4Rep[2][2];
  199.   GrpReg[ 9] = Obj4Rep[3][0];
  200.   GrpReg[10] = Obj4Rep[3][1];
  201.   GrpReg[11] = Obj4Rep[3][2];
  202.   GrpReg[12] = (int)pex;
  203.   GrpReg[13] = (int)pexs;
  204.   Call2Grp(GrpReg,PtsTrans);
  205.   GrpReg[13] = (int)pexs;
  206.   Call2Grp(GrpReg,PtsProj);
  207.  
  208.   for (i=0 ; i<4 ; i++)
  209.     if (pex[i][2] > 0) {
  210.       zneg |= (0x01 << i);
  211.       code  = 0;
  212.       if ( pexs[2*i+1][0] > 319) code |= 0x08;
  213.       if ( pexs[2*i+1][0] <   0) code |= 0x04;
  214.       if ( pexs[2*i+1][1] > 255) code |= 0x02;
  215.       if ( pexs[2*i+1][1] <   0) code |= 0x01;
  216.       codeand &= code;
  217.     }
  218.   if (((codeand != 0) && (zneg == 0x0F)) || (zneg == 0)) return;
  219.  
  220. /* transforme points et normales, puis projette points */
  221.  
  222.   nscreen = sold->nscreen;
  223.   pscreen = sold->pscreen;
  224.  
  225.   GrpReg[ 0] = Obj4Rep[0][0];
  226.   GrpReg[ 1] = Obj4Rep[0][1];
  227.   GrpReg[ 2] = Obj4Rep[0][2];
  228.   GrpReg[ 3] = Obj4Rep[1][0];
  229.   GrpReg[ 4] = Obj4Rep[1][1];
  230.   GrpReg[ 5] = Obj4Rep[1][2];
  231.   GrpReg[ 6] = Obj4Rep[2][0];
  232.   GrpReg[ 7] = Obj4Rep[2][1];
  233.   GrpReg[ 8] = Obj4Rep[2][2];
  234.   GrpReg[12] = (int)(sold->normals);
  235.   GrpReg[13] = (int)nscreen;
  236.   Call2Grp(GrpReg,NormTrans);
  237.  
  238.   GrpReg[ 0] = Obj4Rep[0][0];
  239.   GrpReg[ 1] = Obj4Rep[0][1];
  240.   GrpReg[ 2] = Obj4Rep[0][2];
  241.   GrpReg[ 3] = Obj4Rep[1][0];
  242.   GrpReg[ 4] = Obj4Rep[1][1];
  243.   GrpReg[ 5] = Obj4Rep[1][2];
  244.   GrpReg[ 6] = Obj4Rep[2][0];
  245.   GrpReg[ 7] = Obj4Rep[2][1];
  246.   GrpReg[ 8] = Obj4Rep[2][2];
  247.   GrpReg[ 9] = Obj4Rep[3][0];
  248.   GrpReg[10] = Obj4Rep[3][1];
  249.   GrpReg[11] = Obj4Rep[3][2];
  250.   GrpReg[12] = (int)(sold->points);
  251.   GrpReg[13] = (int)pscreen;
  252.   Call2Grp(GrpReg,PtsTrans);
  253.  
  254.   for(k=0 ; k<tgz-1 ; k++) {
  255.     register int ln,lp;
  256.     ln = k*(tgx-1);
  257.     lp = k* tgx;
  258.     for(i=0 ; i<tgx-1 ; i++) {
  259.       vu  = nscreen[2*(i+ln)  ][0] * pscreen[2*(i+lp)][0];
  260.       vu += nscreen[2*(i+ln)  ][1] * pscreen[2*(i+lp)][1];
  261.       vu += nscreen[2*(i+ln)  ][2] * pscreen[2*(i+lp)][2];
  262.       nscreen[2*(i+ln)  ][3] = -vu;
  263.       if ((pscreen[2*(i  +(k  )*tgx)][2] < 0) ||
  264.           (pscreen[2*(i+1+(k  )*tgx)][2] < 0) ||
  265.           (pscreen[2*(i+1+(k+1)*tgx)][2] < 0)    )
  266.         vu = 1;
  267.       nscreen[2*(i+ln)  ][3] = -vu;
  268.       vu  = nscreen[2*(i+ln)+1][0] * pscreen[2*(i+lp)][0];
  269.       vu += nscreen[2*(i+ln)+1][1] * pscreen[2*(i+lp)][1];
  270.       vu += nscreen[2*(i+ln)+1][2] * pscreen[2*(i+lp)][2];
  271.       if ((pscreen[2*(i  +(k  )*tgx)][2] < 0) ||
  272.           (pscreen[2*(i  +(k+1)*tgx)][2] < 0) ||
  273.           (pscreen[2*(i+1+(k+1)*tgx)][2] < 0)    )
  274.         vu = 1;
  275.       nscreen[2*(i+ln)+1][3] = -vu;
  276.     }
  277.   }
  278.  
  279.   GrpReg[13] = (int)pscreen;
  280.   Call2Grp(GrpReg,PtsProj);
  281.  
  282. /* trace le sol en 4 fois */
  283.  
  284.   InvMat(inv,sol->Rep);
  285.   MkVec3(transl,- sol->Org[0],
  286.                 - sol->Org[1],
  287.                 - sol->Org[2]);
  288.   TransformPt(p,inv,transl,obs->Org);
  289.  
  290.   x   = p[0];
  291.   z   = p[2];
  292.  
  293.   x   = x/(pgx*sold->scalex);
  294.   z   = z/(pgz*sold->scalez);
  295.  
  296.   if ((x>0  ) && (z>0  )) {
  297.     if (x>tgx-2) nx = tgx-2; else nx = x;
  298.     if (z>tgz-2) nz = tgz-2; else nz = z;
  299.     for (k=0 ; k<=nz ; k++) {
  300.       register int ln;
  301.       ln = k*(tgx-1);
  302.       for (i=0 ; i<=nx ; i++) {
  303.        if (nscreen[2*(i+ln)+1][3] > 0) trace(pscreen,i,k,0,1,tgx,sp,pgx,pgz);
  304.        if (nscreen[2*(i+ln)  ][3] > 0) trace(pscreen,i,k,1,0,tgx,sp,pgx,pgz);
  305.       }
  306.     }
  307.   }
  308.  
  309.   if ((x<tgx) && (z>0  )) {
  310.     if (x<0    ) nx = 0    ; else nx = x;
  311.     if (z>tgz-2) nz = tgz-2; else nz = z;
  312.     for (k=0 ; k<=nz ; k++) {
  313.       register int ln;
  314.       ln = k*(tgx-1);
  315.       for (i=tgx-2 ; i>=nx ; i--) {
  316.        if (nscreen[2*(i+ln)  ][3] > 0) trace(pscreen,i,k,1,0,tgx,sp,pgx,pgz);
  317.        if (nscreen[2*(i+ln)+1][3] > 0) trace(pscreen,i,k,0,1,tgx,sp,pgx,pgz);
  318.       }
  319.     }
  320.   }
  321.  
  322.   if ((x>0  ) && (z<tgz)) {
  323.     if (x>tgx-2) nx = tgx-2; else nx = x;
  324.     if (z<0    ) nz = 0    ; else nz = z;
  325.     for (k=tgz-2 ; k>=nz ; k--) {
  326.       register int ln;
  327.       ln = k*(tgx-1);
  328.       for (i=0 ; i<=nx ; i++) {
  329.        if (nscreen[2*(i+ln)+1][3] > 0) trace(pscreen,i,k,0,1,tgx,sp,pgx,pgz);
  330.        if (nscreen[2*(i+ln)  ][3] > 0) trace(pscreen,i,k,1,0,tgx,sp,pgx,pgz);
  331.       }
  332.     }
  333.   }
  334.  
  335.   if ((x<tgx) && (z<tgz)) {
  336.     if (x<0  ) nx = 0  ; else nx = x;
  337.     if (z<0  ) nz = 0  ; else nz = z;
  338.     for (k=tgz-2 ; k>=nz ; k--) {
  339.       register int ln;
  340.       ln = k*(tgx-1);
  341.       for (i=tgx-2 ; i>=nx ; i--) {
  342.        if (nscreen[2*(i+ln)  ][3] > 0) trace(pscreen,i,k,1,0,tgx,sp,pgx,pgz);
  343.        if (nscreen[2*(i+ln)+1][3] > 0) trace(pscreen,i,k,0,1,tgx,sp,pgx,pgz);
  344.       }
  345.     }
  346.   }
  347. }
  348.  
  349. /*****************************************************************************/
  350.  
  351. int CollisionSol(Object *obs,Object *sol,Sol *sold)
  352. {
  353.   mat33  inv;
  354.   vec3   transl,p;
  355.  
  356.   vec4   *points;
  357.   int    tgx,tgz;
  358.   int    i,k;
  359.   double x,z,lx,lz;
  360.   double y;
  361.  
  362.   tgx = sold->tgx;
  363.   tgz = sold->tgz;
  364.   lx  = sold->pgx * sold->scalex;
  365.   lz  = sold->pgz * sold->scalez;
  366.   points = sold->points;
  367.  
  368.   InvMat(inv,sol->Rep);
  369.   MkVec3(transl,- sol->Org[0],
  370.                 - sol->Org[1],
  371.                 - sol->Org[2]);
  372.   TransformPt(p,inv,transl,obs->Org);
  373.  
  374.   i   = (int)(p[0] / lx);
  375.   k   = (int)(p[2] / lz);
  376.   x   =       p[0] % (int)lx;
  377.   z   =       p[2] % (int)lz;
  378. /*
  379.   SetTxtCurPos(15,0);
  380.   printf("%d %d %d %d %d",i,k,x,z,p[1]);
  381. */
  382.   if ((i>=0) && (i<=tgx-2) && (k>=0) && (k<=tgz-2)) {
  383.     if (x>z) {
  384.       y  =  points[i  +(k  )*tgx][1];
  385.       y += (points[i+1+(k  )*tgx][1] - points[i  +(k  )*tgx][1]) * x / lx;
  386.       y += (points[i+1+(k+1)*tgx][1] - points[i+1+(k  )*tgx][1]) * z / lz;
  387.       if (y>p[1]-150) return(y+1); else return(0);
  388.     }
  389.     else {
  390.       y  =  points[i  +(k  )*tgx][1];
  391.       y += (points[i+1+(k+1)*tgx][1] - points[i  +(k+1)*tgx][1]) * x / lx;
  392.       y += (points[i  +(k+1)*tgx][1] - points[i  +(k  )*tgx][1]) * z / lz;
  393.       if (y>p[1]-150) return(y+1); else return(0);
  394.     }
  395.   }
  396.   return(0);
  397. }
  398.  
  399.  
  400.  
  401.  
  402.  
  403.  
  404.  
  405.  
  406.  
  407.  
  408.  
  409.  
  410.