gquadtree.cpp

FreeFem++可以生成高质量的有限元网格。可以用于流体力学

// ORIG-DATE:     Jan 2008
// -*- Mode : c++ -*-
//
// SUMMARY  : Generic Tree (binairy, Quad, Oct)   
// USAGE    : LGPL      
// ORG      : LJLL Universite Pierre et Marie Curi, Paris,  FRANCE 
// AUTHOR   : Frederic Hecht
// E-MAIL   : [e-mail]
//

/*
 
 This file is part of Freefem++
 
 Freefem++ is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.
 
 Freefem++  is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public License
 along with Freefem++; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

 Thank to the ARN ()  FF2A3 grant
 ref:ANR-07-CIS7-002-01 
 */


#include <cmath>
#include <cstdlib>
#include "error.hpp"
#include <iostream>
#include <climits>
#include  <cfloat>
#include <cstring>
#include "ufunction.hpp"
#include "HeapSort.hpp"
using namespace std;


#include "GenericMesh.hpp"
#include "Mesh1dn.hpp"
#include "Mesh2dn.hpp"
#include "Mesh3dn.hpp"





namespace EF23 {
  
  //  new version ----------
  //  ----------------------
  template<class Rd>
  void OrthoProject(const Rd &P,const Rd &A,const Rd & B,R * l)
  {
    Rd AB(A,B),AP(A,P),BP(B,P);
    R pa=(AB,AP),pb=(AB,BP);
    //  (l0 PA + l1 PB , AB)=0
    //   l0 pa + l1 pb = 0
    //   l0 + l1 =1
    //   l0 (pa - pb) = - pb;
    l[0] = - pb/(pa-pb);
    l[1] = 1-l[0];
  }
  
  template<class Rd>
  void OrthoProject(const Rd &P,const Rd &A,const Rd &B,const Rd &C,R * l)
  {
  Rd AB(A,B),AC(A,C),AP(A,P),BP(B,P),CP(C,P);
  R2 p0( (AB,AP) , (AC,AP) );
  R2 p1( (AB,BP) , (AC,BP) );
  R2 p2( (AB,CP) , (AC,CP) );
  // sum  li pi = 0
  //   l0    + l1    + l2     = 1
  //  =>     
  R2 O;
  R d = det(p0,p1,p2);
  l[0] = det(O ,p1,p2)/ d;
  l[1] = det(p0,O ,p2)/ d;
  l[2] = 1. -l[0] -l[1];
  //  
  //     
  }
  
  template<class Vertex>    
  Vertex *  GTree<Vertex>::NearestVertex(Zd xyi)//long xi,long yj)
  {
    // warning this function return the NearestVertex in the first
    // none empty box contening the point xyi.
    //   They do not return the true nearest point in classical norm.
      
    QuadTreeBox * pb[ MaxDeep ];
    int  pi[ MaxDeep  ];
    Zd pp[  MaxDeep ];
    int l=0; // level
    QuadTreeBox * b;
    IntQuad  h=MaxISize,h0;
    IntQuad hb =  MaxISize;
    Zd   p0(0,0,0);
    Zd  plus(xyi) ; plus.Bound();
    
    // xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
    
    Vertex *vn=0;
    
    // init for optimisation ---
    b = root;
    long  n0;
    if (!root->n)
      return vn; // empty tree 
    
    while( (n0 = b->n) < 0) 
      {
		// search the non empty 
		// QuadTreeBox containing  the point (i,j)
		long hb2 = hb >> 1 ;
		int k = plus.Case(hb2);//(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j

		QuadTreeBox * b0= b->b[k];
 
		if ( ( b0 == 0) || (b0->n == 0) ){
		  break; // null box or empty box   => break 
		}   
		NbQuadTreeBoxSearch++;
		b=b0;
		p0.Add(k,hb2);
		hb = hb2; 
		
      }
      // n0 number of boxes of in b ("b0")
    if(verbosity>200)
    cout << "n0=" << n0 << endl;
    
    if ( n0 > 0) 
    {  
      for( int k=0;k<n0;k++)
	{
	  Zd i2 =  VtoZd(b->v[k]);
	  h0 = Zd(i2,plus).norm();  //NORM(iplus,i2.x,jplus,i2.y);
	  if (h0 <h) {
	    h = h0;
	    vn = b->v[k];
	  }
	  NbVerticesSearch++;
	}
      
      return vn;
    }
    
if(verbosity>200)
    cout << "general case : NearVertex" << endl; 
    // general case -----
  pb[0]= b;
  pi[0]=b->n>0 ?(int)  b->n : N  ;
  pp[0]=p0;
  h=hb;
  do {    
    b= pb[l];
    while (pi[l]--)
      { 	      
        int k = pi[l];
	
	if (b->n>0) // Vertex QuadTreeBox none empty
	  { 
	    NbVerticesSearch++;
	    Zd i2 =  VtoZd(b->v[k]);
	    h0 = Zd(i2,plus).norm();//  NORM(iplus,i2.x,jplus,i2.y);
	    if (h0 <h) 
	      {
		h = h0;
		vn = b->v[k];
	      }
	  }
	else // Pointer QuadTreeBox 
	  { 
	    QuadTreeBox *b0=b;
	    NbQuadTreeBoxSearch++;
	    if ((b=b->b[k]))
	      {
		hb >>=1 ; // div by 2
		Zd ppp(pp[l],k,hb);
		
		if  ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h)) 
		  {
		    pb[++l]=  b;
		    pi[l]= b->n>0 ?(int)  b->n : N  ;
		    pp[l]=ppp;
		  }
		else
		  b=b0, hb <<=1 ;
	      }
	    else
	      b=b0;
	  }
      }
    hb <<= 1; // mul by 2 
  } while (l--);
  
  return vn;
  }
  
  
  template<class Vertex>
  Vertex *  GTree<Vertex>::ToClose(const Rd & v,R seuil,Zd H)
  {
    const Rd X(v);
    const Zd p(RdtoZd(v));
    R seuil2 = seuil*seuil;
    // const Metric  Mx(v.m);
    
    QuadTreeBox * pb[ MaxDeep ];
    int  pi[ MaxDeep  ];
    Zd pp[  MaxDeep ];
    
    int l=0; // level
    QuadTreeBox * b;
    long h=MaxISize;
    long hb =  MaxISize;
    Zd p0( 0 );
    
    if (!root->n)
      return 0; // empty tree 
   
    // general case -----
    pb[0]= root;
    pi[0]= root->n>0 ?(int)  root->n : N ;
    pp[0]= p0;
    h=hb;
    do {    
      b= pb[l];
      while (pi[l]--)
	{ 	      
	  int k = pi[l];
	  //cout << "b" << b << ", k= " << k << endl;
	  //cout << " b->n " << b->n << endl;
	  if (b->n>0) // Vertex QuadTreeBox none empty
	    { 
	      NbVerticesSearch++;
	      Vertex & V(*b->v[k]);
	      Zd i2 =  VtoZd(V);
	   
	      if ( Zd(i2,p).less(H) )
		{
		  
		  Rd XY(X,V);
		  R dd;
		  if( (dd= (XY,XY) ) < seuil2 ) // LengthInterpole(Mx(XY), b->v[k]->m(XY)))  < seuil )
		    { 
		
		      return &V; 
		    }
		}
	    }
	  else // Pointer QuadTreeBox 
	    { 
	      QuadTreeBox *b0=b;
	      NbQuadTreeBoxSearch++;
	      if( (b=b->b[k]) ) 
		{
			
		  hb >>=1; // div by 2
		  Zd ppp(pp[l],k,hb);
		  	
		  if( ppp.interseg(p,hb,H) )
		    {
		      	
		      pb[++l]=  b;
		      pi[l]= b->n>0 ?(int)  b->n : N;  
		      pp[l]=ppp;
		    }
		  else
		    b=b0, hb <<=1 ;
		    	
		}
	      else
		b=b0;
	    }
	} // fin: while(pi[l]--)
      hb <<= 1; // mul by 2 
    } while (l--);

    return 0;
    
  }
  
  template<class Vertex>
  void  GTree<Vertex>::Add( Vertex & w)
  {
    QuadTreeBox ** pb , *b;
    Zd p(VtoZd(w));
    long l=MaxISize;
    pb = &root;
   
    while( (b=*pb) && (b->n<0))
      { 
	b->n--;
	l >>= 1;
	pb = &b->b[p.Case(l)];
      }
    if  (b) {
      for(int i=N-1;i>=0;--i)
	if (b->n > i &&  b->v[i] == &w)
	  {
	    //if( abs(w.x+0.5)<1e-10 ) cout << "if (b->n > i &&  b->v[i] == &w)" <<endl;
	    return;
	  }
    }
    assert(l);
    while ((b= *pb) && (b->n == N)) // the QuadTreeBox is full
      { 
	//if(verbosity > 5) cout << " b = " << b << b->n << "  " << l << endl;
	Vertex *v4[N]; // copy of the QuadTreeBox vertices      
	for(int i=0;i<N;++i)
	  { v4[i]= b->v[i];
	  b->v[i]=0;}
	
	b->n = -b->n; // mark is pointer QuadTreeBox
	
	l >>= 1;    // div the size by 2
	ffassert(l);
	for (int k=0;k<N;k++) // for the 4 vertices find the sub QuadTreeBox ij
	  { 
	    int ij;
	    QuadTreeBox * bb =  b->b[ij=VtoZd(v4[k]).Case(l)];
	    //if(verbosity > 5)  cout << "ij= "<< ij <<  " " << VtoZd(v4[k])<< endl;
	    if (!bb) 
	      bb=b->b[ij]=NewQuadTreeBox(); // alloc the QuadTreeBox 
	    //if(verbosity > 5)  cout << bb << " " << k << " "  << ij <<  endl;
	    bb->v[bb->n++] = v4[k];
	  }
	pb = &b->b[p.Case(l)];
      }
    if (!(b = *pb))
      { //if(verbosity > 5)  cout << "Qbox \n";
	b=*pb= NewQuadTreeBox(); //  alloc the QuadTreeBox 
      }
    //if(verbosity > 5)  cout << b << " " << b->n << endl;
    b->v[b->n++]=&w; // we add the vertex 
    NbVertices++;    
    
  }

    
template<class Vertex>    
GTree<Vertex>::GTree(Vertex * v,Rd Pmin,Rd Pmax,int nbv) : 
 lenStorageQuadTreeBox(nbv/8+100),
 // th(t),
 NbQuadTreeBoxSearch(0),
 NbVerticesSearch(0),
 NbQuadTreeBox(0),
 NbVertices(0),
 cMin(Pmin-(Pmax-Pmin)/2),
 cMax(Pmax+(Pmax-Pmin)/2),
 coef( MaxISize/Norme_infty(cMax-cMin))
 
{ 
  if(verbosity>5){
    cout << "  GTree: box: "<<  Pmin << " " << Pmax << " " << cMin << " "<< cMax << " nbv : " << nbv <<endl;
    cout << "MaxISize " << MaxISize << endl; 
    //cout << "  RdtoZd(cMin)" << RdtoZd(cMin) << " RdtoZd(cMax)" << RdtoZd(cMax) << endl;
    //cout << "  RdtoZd(Pmin)" << RdtoZd(Pmin) << " RdtoZd(Pmax)" << RdtoZd(Pmax) << endl;
    }
  sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
  root=NewQuadTreeBox();
  //  throwassert( MaxISize > MaxICoor);
  if (v)
    for (long i=0;i<nbv;i++) 
      Add(v[i]);
}
  
  template<class Vertex>
  GTree<Vertex>::GTree() : 
  lenStorageQuadTreeBox(100),
  // th(0),
  NbQuadTreeBoxSearch(0),
  NbVerticesSearch(0),
  NbQuadTreeBox(0),
  NbVertices(0),
  cMin(),cMax(),coef(0)
  {
    sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
    root=NewQuadTreeBox();
  }
  
  template<class Vertex>    
  GTree<Vertex>::StorageQuadTreeBox::StorageQuadTreeBox(int ll,StorageQuadTreeBox *nn)
  {
    len = ll;
    n = nn;
    b = new QuadTreeBox[ll];
    for (int i = 0; i <ll;i++)
      {
	b[i].n =0;
	for(int j=0;j<N;++j)
	  b[i].b[j]=0;
      }