stream.c

来自「FreeFem++可以生成高质量的有限元网格。可以用于流体力学」· C语言 代码 · 共 1,962 行 · 第 1/4 页

#include "medit.h"
#include "extern.h"
#include "sproto.h"

#define MAX_PTS   100000
#define MAX_CPT   5000
#define MAX_LST   1024

#define COS175  -0.996194698
#define COS178  -0.99939083
#define HSIZ     0.03
#define EPST    -1.e-14
#define EPSR     1.e+14


extern int       reftype,refitem;
extern GLfloat   altcoef;
int    nbar=0;
enum   {Euler=1,RK4=2};


/* find tetra containg p, starting nsdep */
int locateTetra(pMesh mesh,int nsdep,int base,float *p,double *cb) {
  pTetra   pt;
  pPoint   p0,p1,p2,p3;
  double   bx,by,bz,cx,cy,cz,dx,dy,dz,vx,vy,vz,apx,apy,apz;
  double   epsra,vol1,vol2,vol3,vol4,dd; 
  int     *adj,iadr,it,nsfin;

  it    = 0;
  nsfin = nsdep;
  /*  printf("locateTetra: searching for %f %f %f\n",p[0],p[1],p[2]);*/
  do {
    if ( !nsfin )  return(0);
    pt = &mesh->tetra[nsfin];
    if ( !pt->v[0] )  return(0);
    if ( pt->mark == base )  return(0);
    pt->mark = base;
    iadr = 4*(nsfin-1)+1;
    adj  = &mesh->adja[iadr];
    p0 = &mesh->point[pt->v[0]];
    p1 = &mesh->point[pt->v[1]];
    p2 = &mesh->point[pt->v[2]];
    p3 = &mesh->point[pt->v[3]];

    /* barycentric */
    bx  = p1->c[0] - p0->c[0];
    by  = p1->c[1] - p0->c[1];
    bz  = p1->c[2] - p0->c[2];
    cx  = p2->c[0] - p0->c[0];
    cy  = p2->c[1] - p0->c[1];
    cz  = p2->c[2] - p0->c[2];
    dx  = p3->c[0] - p0->c[0];
    dy  = p3->c[1] - p0->c[1];
    dz  = p3->c[2] - p0->c[2];

    /* test volume */
    vx  = cy*dz - cz*dy;
    vy  = cz*dx - cx*dz;
    vz  = cx*dy - cy*dx;

    epsra = EPST*(bx*vx + by*vy + bz*vz);
    apx = p[0] - p0->c[0];
    apy = p[1] - p0->c[1];
    apz = p[2] - p0->c[2];

    /* p in 2 */
    vol2  = apx*vx + apy*vy + apz*vz;
    if ( epsra > vol2 ) {
      nsfin = adj[1];
      continue;
    }

    /* p in 3 */
    vx  = by*apz - bz*apy;
    vy  = bz*apx - bx*apz;
    vz  = bx*apy - by*apx;
    vol3 = dx*vx + dy*vy + dz*vz;
    if ( epsra > vol3 ) {
      nsfin = adj[2];
      continue;
    }
    
    /* p in 4 */
    vol4 = -cx*vx - cy*vy - cz*vz;
    if ( epsra > vol4 ) {
      nsfin = adj[3];
      continue;
    }
    
    /* p in 1 */
    vol1 = -epsra * EPSR - vol2 - vol3 - vol4;
    if ( epsra > vol1 ) {
      nsfin = adj[0];
      continue;
    }

    dd = vol1+vol2+vol3+vol4;
    if ( dd != 0.0 )  dd = 1.0 / dd;
    cb[0] = vol1 * dd;
    cb[1] = vol2 * dd;
    cb[2] = vol3 * dd;
    cb[3] = vol4 * dd; 
    
    pt->cpt++;
    return(nsfin);
  }
  while ( ++it <= mesh->ntet );

  return(0);
}


/* return 1 if point in tetra, adjacent #, if not */
int inSubTetra(pPoint pt[4],float *p,double *cb) {
  double   bx,by,bz,cx,cy,cz,dx,dy,dz,vx,vy,vz,apx,apy,apz;
  double   epsra,vol1,vol2,vol3,vol4,dd; 

  /* barycentric */
  bx  = pt[1]->c[0] - pt[0]->c[0];
  by  = pt[1]->c[1] - pt[0]->c[1];
  bz  = pt[1]->c[2] - pt[0]->c[2];
  cx  = pt[2]->c[0] - pt[0]->c[0];
  cy  = pt[2]->c[1] - pt[0]->c[1];
  cz  = pt[2]->c[2] - pt[0]->c[2];
  dx  = pt[3]->c[0] - pt[0]->c[0];
  dy  = pt[3]->c[1] - pt[0]->c[1];
  dz  = pt[3]->c[2] - pt[0]->c[2];

  /* test volume */
  vx  = cy*dz - cz*dy;
  vy  = cz*dx - cx*dz;
  vz  = cx*dy - cy*dx;

  epsra = EPST*(bx*vx + by*vy + bz*vz);
  apx = p[0] - pt[0]->c[0];
  apy = p[1] - pt[0]->c[1];
  apz = p[2] - pt[0]->c[2];

  /* p in 2 */
  vol2 = apx*vx + apy*vy + apz*vz;
  if ( epsra > vol2 )  return(-1);

  /* p in 3 */
  vx  = by*apz - bz*apy;
  vy  = bz*apx - bx*apz;
  vz  = bx*apy - by*apx;
  vol3 = dx*vx + dy*vy + dz*vz;
  if ( epsra > vol3 )  return(-2);
  
  /* p in 4 */
  vol4 = -cx*vx - cy*vy - cz*vz;
  if ( epsra > vol4 )  return(-3);

  /* p in 1 */
  vol1 = -epsra * EPSR - vol2 - vol3 - vol4;
  if ( epsra > vol1 )  return(0);

  dd = vol1+vol2+vol3+vol4;
  if ( dd != 0.0f )  dd = 1.0 / dd;
  cb[0] = vol1 * dd;
  cb[1] = vol2 * dd;
  cb[2] = vol3 * dd;
  cb[3] = vol4 * dd; 
  
  return(1);
}

int locateHexa(pMesh mesh,int nsdep,int base,float *p,double *cb,pPoint pt[4]) {
  pHexa    ph;
  double   bx,by,bz,cx,cy,cz,dx,dy,dz,vx,vy,vz,apx,apy,apz;
  double   epsra,vol1,vol2,vol3,vol4,dd; 
  int     *adj,iadr,it,nsfin,in;

  it    = 0;
  nsfin = nsdep;
  /*printf("locateHexa: searching for %f %f %f\n",p[0],p[1],p[2]);*/
  do {
    if ( !nsfin )  return(0);
    ph = &mesh->hexa[nsfin];
/*printf("\nnsfin %d  base %d  mark %d\n",nsfin,base,ph->mark);*/
    if ( !ph->v[0] || ph->mark == base )  return(0);
    ph->mark = base;
    iadr = 6*(nsfin-1)+1;
    adj  = &mesh->adja[iadr];
/*printf("adj %d %d %d %d %d %d\n",adj[0],adj[1],adj[2],adj[3],adj[4],adj[5]);*/

    /* tetra1: 0,2,3,7 : 3 external faces */
/*printf("tet1: %d %d %d %d\n",ph->v[0],ph->v[2],ph->v[3],ph->v[7]);*/
    pt[0] = &mesh->point[ph->v[0]];
    pt[1] = &mesh->point[ph->v[2]];
    pt[2] = &mesh->point[ph->v[3]];
    pt[3] = &mesh->point[ph->v[7]];
    in = inSubTetra(pt,p,cb);
printf("tet1 : on sort en %d\n",in);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == 0 ) {
      nsfin = adj[4];
      continue;
    }
    else if ( in == -1 ) {
      nsfin = adj[5];
      continue;
    }
    else if ( in == -3 ) {
      nsfin = adj[0];
      continue;
    }

    /* tetra2: 1,4,5,6 : 3 external faces */
    pt[0] = &mesh->point[ph->v[1]];
    pt[1] = &mesh->point[ph->v[4]];
    pt[2] = &mesh->point[ph->v[5]];
    pt[3] = &mesh->point[ph->v[6]];
    in = inSubTetra(pt,p,cb);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == 0 ) {
      nsfin = adj[1];
      continue;
    }
    else if ( in == -1 ) {
      nsfin = adj[3];
      continue;
    }
    else if ( in == -3 ) {
      nsfin = adj[2];
      continue;
    }

    /* tetra3: 0,4,6,7 : 2 external faces */
    pt[0] = &mesh->point[ph->v[0]];
    pt[1] = &mesh->point[ph->v[4]];
    pt[2] = &mesh->point[ph->v[6]];
    pt[3] = &mesh->point[ph->v[7]];
    in = inSubTetra(pt,p,cb);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == 0 ) {
      nsfin = adj[1];
      continue;
    }
    else if ( in == -2 ) {
      nsfin = adj[5];
      continue;
    }

    /* tetra4: 0,1,2,6 : 2 external faces */
    pt[0] = &mesh->point[ph->v[0]];
    pt[1] = &mesh->point[ph->v[1]];
    pt[2] = &mesh->point[ph->v[2]];
    pt[3] = &mesh->point[ph->v[6]];
    in = inSubTetra(pt,p,cb);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == 0 ) {
      nsfin = adj[3];
      continue;
    }
    else if ( in == -3 ) {
      nsfin = adj[0];
      continue;
    }
    
    /* tetra5: 0,6,2,7 : 1 external face */
    pt[0] = &mesh->point[ph->v[0]];
    pt[1] = &mesh->point[ph->v[6]];
    pt[2] = &mesh->point[ph->v[2]];
    pt[3] = &mesh->point[ph->v[7]];
    in = inSubTetra(pt,p,cb);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == 0 ) {
      nsfin = adj[4];
      continue;
    }

    /* tetra6: 0,4,1,6 : 1 external face */
    pt[0] = &mesh->point[ph->v[0]];
    pt[1] = &mesh->point[ph->v[4]];
    pt[2] = &mesh->point[ph->v[1]];
    pt[3] = &mesh->point[ph->v[6]];
    in = inSubTetra(pt,p,cb);
    if ( in > 0 ) {
      ph->cpt++;
      return(nsfin);
    }
    else if ( in == -3 ) {
      nsfin = adj[2];
      continue;
    }
puts("PROBLEME");
exit(1);
  }
  while ( ++it <= mesh->nhex );

  return(0);
}

int locateTria(pMesh mesh,int nsdep,int base,float *p,double *cb) {
  pTriangle pt;
  pPoint    p0,p1,p2;
  double    ax,ay,bx,by,cx,cy;
  double    epsra,aire1,aire2,aire3,dd; 
  int      *adj,iadr,it,isign,nsfin;

  it    = 0;
  nsfin = nsdep;
  /*printf("locateTria: searching for %f %f\n",p[0],p[1]);*/
  do {
    pt = &mesh->tria[nsfin];
    if ( !pt->v[0] )  return(0);
    if ( pt->mark == base )  return(0);
    pt->mark = base;
    iadr = 3*(nsfin-1)+1;
    adj  = &mesh->adja[iadr];

    p0 = &mesh->point[pt->v[0]];
    p1 = &mesh->point[pt->v[1]];
    p2 = &mesh->point[pt->v[2]];

    ax = p1->c[0] - p0->c[0];
    ay = p1->c[1] - p0->c[1];
    bx = p2->c[0] - p0->c[0];
    by = p2->c[1] - p0->c[1];
    dd = ax*by - ay*bx;
    isign= dd > 0 ? 1 : -1;
    epsra = isign > 0 ? EPST*dd : -(EPST*dd);
    /* barycentric */
    bx = p1->c[0] - p[0];
    by = p1->c[1] - p[1];
    cx = p2->c[0] - p[0];
    cy = p2->c[1] - p[1];
    /* p in 1 */
    aire1 = isign*(bx*cy - by*cx);
    if ( epsra > aire1 ) {
      nsfin = adj[0];
      continue;
    }

    ax = p0->c[0] - p[0];
    ay = p0->c[1] - p[1];
    aire2 = isign*(cx*ay - cy*ax);
    if ( epsra > aire2 ) {
      nsfin = adj[1];
      continue;
    }

    aire3 = -epsra*EPSR - aire1 - aire2;
    if ( epsra > aire3 ) {
      nsfin = adj[2];
      continue;
    }

    dd = aire1+aire2+aire3;
    if ( dd != 0.0f )  dd = 1.0 / dd;
    cb[0] = aire1 * dd;
    cb[1] = aire2 * dd;
    cb[2] = aire3 * dd;
    
    pt->cpt++;
    return(nsfin);
  }
  while ( ++it <= mesh->nt );

  return(0);
}

/* point in tetra */
int inTetra(pMesh mesh,int nsdep,float *p,double *cb) {
  pTetra   pt;
  pPoint   p0,p1,p2,p3;
  double   bx,by,bz,cx,cy,cz,dx,dy,dz,vx,vy,vz,apx,apy,apz;
  double   epsra,vol1,vol2,vol3,vol4,dd; 

  pt = &mesh->tetra[nsdep];
  if ( !pt->v[0] )  return(0);

  p0 = &mesh->point[pt->v[0]];
  p1 = &mesh->point[pt->v[1]];
  p2 = &mesh->point[pt->v[2]];
  p3 = &mesh->point[pt->v[3]];

  /* barycentric */
  bx  = p1->c[0] - p0->c[0];
  by  = p1->c[1] - p0->c[1];
  bz  = p1->c[2] - p0->c[2];
  cx  = p2->c[0] - p0->c[0];
  cy  = p2->c[1] - p0->c[1];
  cz  = p2->c[2] - p0->c[2];
  dx  = p3->c[0] - p0->c[0];
  dy  = p3->c[1] - p0->c[1];
  dz  = p3->c[2] - p0->c[2];

  /* test volume */
  vx  = cy*dz - cz*dy;
  vy  = cz*dx - cx*dz;
  vz  = cx*dy - cy*dx;

  epsra = EPST*(bx*vx + by*vy + bz*vz);
  apx = p[0] - p0->c[0];
  apy = p[1] - p0->c[1];
  apz = p[2] - p0->c[2];

  /* p in 2 */
  vol2  = apx*vx + apy*vy + apz*vz;
  if ( epsra > vol2 )  return(0);

  /* p in 3 */
  vx  = by*apz - bz*apy;
  vy  = bz*apx - bx*apz;
  vz  = bx*apy - by*apx;
  vol3 = dx*vx + dy*vy + dz*vz;
  if ( epsra > vol3 )  return(0);
    
  /* p in 4 */
  vol4 = -cx*vx - cy*vy - cz*vz;
  if ( epsra > vol4 )  return(0);
  
  /* p in 1 */
  vol1 = -epsra * EPSR - vol2 - vol3 - vol4;
  if ( epsra > vol1 )  return(0);

  dd = vol1+vol2+vol3+vol4;
  if ( dd != 0.0f )  dd = 1.0 / dd;
  cb[0] = vol1 * dd;
  cb[1] = vol2 * dd;
  cb[2] = vol3 * dd;
  cb[3] = vol4 * dd; 

  pt->cpt++;
  return(1);
}

int inHexa(pMesh mesh,int nsdep,float *p,double *cb,pPoint pt[4]) {
  return(0);
}

int inTria(pMesh mesh,int nsdep,float *p,double *cb) {
  pTriangle  pt;
  pPoint     p0,p1,p2;
  double     ax,ay,bx,by,cx,cy;
  double     epsra,dd,aire1,aire2,aire3;
  int        isign;

  pt = &mesh->tria[nsdep];
  if ( !pt->v[0] )  return(0);

  p0 = &mesh->point[pt->v[0]];
  p1 = &mesh->point[pt->v[1]];
  p2 = &mesh->point[pt->v[2]];

  ax = p1->c[0] - p0->c[0];
  ay = p1->c[1] - p0->c[1];
  bx = p2->c[0] - p0->c[0];
  by = p2->c[1] - p0->c[1];
  dd = ax*by - ay*bx;
  isign= dd > 0 ? 1 : -1;
  epsra = isign > 0 ? EPST*dd : -(EPST*dd);

  /* barycentric */
  bx = p[0] - p1->c[0];
  by = p[1] - p1->c[1];
  cx = p[0] - p2->c[0];
  cy = p[1] - p2->c[1];
  aire1 = isign*(bx*cy - by*cx);
  if ( epsra > aire1 )  return(0);

  ax = p[0] - p0->c[0];
  ay = p[1] - p0->c[1];
  aire2 = isign*(cx*ay - cy*ax);
  if ( epsra > aire2 )  return(0);
  
  aire3 = -epsra*EPSR - aire1 - aire2;
  if ( epsra > aire3 )  return(0);

  dd = aire1+aire2+aire3;
  if ( dd != 0.0f )  dd = 1.0 / dd;
  cb[0] = aire1 * dd;
  cb[1] = aire2 * dd;