/Users/mourrain/Devel/mmx/shape/src/ssiqts.cpp

Go to the documentation of this file.

# include <shape/ssiqts.hpp>
# include <shape/surface_parametric.hpp>
# define  ParametricSurface shape::surface_parametric<double>

namespace mmx {
#include <shape/ssiqts_offsets.hpp>

static bool swap;

SSIQTS::sample::sample( ParametricSurface * s, int m, int n )
{
  m_uvals = new double[3*m*n+m+n];
  m_vvals = m_uvals + m;
  m_svals = (fxv<double,3>*)(m_vvals + n);
  m_nrows = m;
  m_ncols = n;
  swap = true; s->sample((double*)m_svals,m,n,m_uvals,m_vvals);
  //  for ( int i = 0; i < m*n; i ++ ) std::cout << m_svals[i]  << std::endl;
};

SSIQTS::sample::~sample() { delete[] m_uvals; };

static SSIQTS::aabb3 * alloc( int& l, int m, int& s )
{
  int c = m*m;
  s = 0;
  l = 0;
  while ( c ) { s += c; c /= 4; l ++;  };
  return new SSIQTS::aabb3[s];
};

static void fillpatchbox( SSIQTS::aabb3& b, const SSIQTS::vector3 * a, SSIQTS& ssi )
{
  //  unsigned nrows = ssi.m*ssi.udeg+1;
  unsigned ncols = ssi.m*ssi.vdeg+1;
  //  unsigned srows = ssi.udeg*ncols;      
  const SSIQTS::vector3 *  la = a;
  const SSIQTS::vector3 * ela = a + (ssi.udeg+1)*ncols;
  const SSIQTS::vector3 * ca;
  for ( int d = 0; d < 3;  d ++ ) b[d][0] = b[d][1] = (*a)[d];
  for ( ca = a+1; ca != la + ssi.vdeg+1; ca++ )
    for ( int d = 0; d < 3; d ++ )
      {
        if ( (*ca)[d] < b[d][0] ) b[d][0] = (*ca)[d];
        else if ( (*ca)[d] > b[d][1]) b[d][1] = (*ca)[d];
      };
  
  for ( la = a+ncols; la != ela; la += ncols )
    for ( ca = la; ca != la + ssi.vdeg+1; ca ++ )
      for ( int d = 0; d < 3; d ++ )
        {
          if ( (*ca)[d] < b[d][0] ) b[d][0] = (*ca)[d];
          else if ( (*ca)[d] > b[d][1] ) b[d][1] = (*ca)[d];
        };
};


static void fillboxes( SSIQTS::aabb3 * qta, const SSIQTS::vector3 * base, SSIQTS& ssi )
{
  unsigned nrows = ssi.m*ssi.udeg+1;
  unsigned ncols = ssi.m*ssi.vdeg+1;
  unsigned srows = ssi.udeg*ncols;
  const SSIQTS::vector3 * lptr  = base;
  const SSIQTS::vector3 * cptr;
  const SSIQTS::vector3 * elptr = base + nrows*(ncols-1);
  for ( lptr = base; lptr != elptr; lptr += srows )
    for ( cptr = lptr; cptr != lptr + ncols-1; cptr += ssi.vdeg, qta++  )
      fillpatchbox( *qta, cptr, ssi );
};

static bool intersectp( const SSIQTS::aabb3 & a, const SSIQTS::aabb3 & b )
{
  for ( int i = 0; i < 3; i ++ ) 
    if ( a[i][1] < b[i][0] || b[i][1] < a[i][0] ) return false;
  return true;
};

static void merge( SSIQTS::aabb3 & box, const SSIQTS::aabb3& a, const SSIQTS::aabb3 & b )
{
  for ( unsigned i = 0; i < 3; i ++ )
    {
      box[i][0] = std::min(a[i][0],b[i][0]);
      box[i][1] = std::max(a[i][1],b[i][1]);
    };
};

static void update( SSIQTS::aabb3& box, const SSIQTS::aabb3& a, const SSIQTS::aabb3& b )
{
  double tmp;
  for ( int i = 0; i < 3; i ++ )
    {
      tmp = std::min(a[i][0],b[i][0]);
      if ( tmp < box[i][0] ) box[i][0] = tmp;
      tmp = std::max(a[i][1],b[i][1]);
      if ( tmp > box[i][1] ) box[i][1] = tmp;
    };
};

static void build( SSIQTS::aabb3 * base, const SSIQTS & ssi )
{
  int N = ssi.m;
  SSIQTS::aabb3 * cptr,*eptr,*next,*enext;
  cptr  = base;
  eptr  = base + N;
  next  = base + N*N;
  enext = next + (N/2*N/2);
  
  while ( next < enext )
    {
      base = next;
      do
        {
          for ( ;cptr < eptr; cptr += 2, next++ )
            {  
              merge(*next,*cptr,*(cptr+1));
              update(*next,*(cptr+N),*(cptr+1+N));
            };
          
          cptr += N;
          eptr  = cptr + N;
        }
      while ( next < enext );
      cptr = base;
      N /= 2;
      eptr = cptr + N;
      enext = next + (N/2)*(N/2);
    };
};
  

/* tests de collision sur un niveau de subdivision */
static int cfhunt( 
                  SSIQTS::aabb3 * lbb, /* ensemble des boites de la surface "gauche" au niveau actuel de subdivision */
                  SSIQTS::aabb3 * rbb, /* ensemble des boites de la surface "droite" au niveau actuel de subdivision */
                  int * bcf,   /* debut de la liste des conflits */
                  int * ecf )  /* fin   de la liste des conflits */
{
  int s = 0;
  int * rqp;  // right-quad pointer
  int *  cf;
  int * lrqp; // the last right-quad pointer...
  
  /* pour tous les conflits encore presents a la resolution actuelle */
  for ( cf = bcf; cf != ecf; cf += *cf ) 
    {
      const int lq = cf[cflq_];              // addresse de l'element de la surface "gauche"
      lrqp = cf+cfbg_;                       // debut de la liste des elements de "droite"
       /* pour tout les elements de droite test d'intersection avec l'element de gauche */
       for ( rqp = lrqp; rqp != cf + *cf; rqp ++ ) 
         {
           /* si les boites s'intersectent, tassement de la liste  */
           if ( intersectp(*(lbb+lq),*(rbb+*rqp)) ) { *lrqp++ = *rqp; };
         };
       if ( lrqp != rqp ) *lrqp = -1; // marque la fin de liste
       int nc = lrqp-(cf+cfbg_);          // nombre de collisions restantes
       s += (nc)?4*(cfhsz+4*nc):0;        // taille necessaire pour leurs subdivisions
    };
  return s; // taille totale necessaire apres traitement d'un niveau de subdivision
}


static int * cfsimplify( int * bcf, int * ecf )
{
  int * necf = bcf;
  for ( int * cf = bcf; cf != ecf; )
    {
      int * rp = cf+cfbg_;
      //      int i = 0;
      int * dst = necf+cfbg_;
      while( *rp != -1 && rp < cf + *cf ) *dst++ = *rp++;
      necf[cflq_] = cf[cflq_];
      
      cf += *cf;
      *necf = dst-necf;
      necf += *necf;
    };
  return necf;
};

static  int * expandcf( int * cf, int * ncf, int m )
{
  int * ecf, * rpq, *pncf;
  ecf = cf + cf[cfsz_];
  rpq = cf + cfbg_;
  while ( *rpq != -1 && rpq != ecf ) rpq++;
  if ( rpq == cf + cfbg_ ) return ncf;
  *ncf = 4*(rpq-(cf+cfbg_))+cfhsz;
  rpq = cf + cfbg_;
  for ( pncf = ncf + cfbg_; pncf != ncf + *ncf; pncf += 4 , rpq ++  )
    {
      int i =  *rpq / m;
      int j =  *rpq % m;
      int a =  i*(4*m)+2*j;
      pncf[0] = a;//2**rpq;
      pncf[1] = a+1;//2**rpq+1;
      pncf[2] = a+2*m;//2*(*rpq+m);
      pncf[3] = a+2*m+1;//2*(*rpq+m)+1;
    };
  // they are all the same 
  for ( pncf = ncf + *ncf; pncf != ncf + 4**ncf; pncf += *ncf )
    std::copy(ncf,ncf+*ncf,pncf);
  // with a change on the left quad ...
  int i =  cf[cflq_]/ m;
  int j =  cf[cflq_]% m;
  int a =  i*(4*m)+2*j;
  (ncf+0**ncf)[cflq_] = a;//2*cf[1];
  (ncf+1**ncf)[cflq_] = a+1;//2*cf[1]+1;
  (ncf+2**ncf)[cflq_] = a+2*m;//2*(cf[1]+m);
  (ncf+3**ncf)[cflq_] = a+2*m+1;//2*(cf[1]+m)+1;
  return ncf + 4**ncf;
};

static void cfforward( int * ncf, int * bcf, int * ecf, int m )
{
  for ( int * cf = bcf; cf != ecf; ncf = expandcf(cf,ncf,m), cf += *cf  );
};


static void search( SSIQTS::aabb3 * lroot, SSIQTS::aabb3 * rroot, SSIQTS & ssi  )
{
  int S, M;
  int *  bcf, * ecf, * ncf;
  
  // nxt,ext,lq,b0,b1...
  /* conflits de depart: (0, 0) */
  bcf = new int[cfhsz+1];
  bcf[cfsz_]   = cfhsz+1;
  bcf[cflq_]   = 0;
  bcf[cfbg_]   = 0;
  ecf = bcf + bcf[cfsz_];
  int s = 0;
  
  SSIQTS::aabb3 *lbb = lroot; /* represente le niveau courant dans le quadtree "gauche" */
  SSIQTS::aabb3 *rbb = rroot; /* represente le niveau courant dans le quadtree "droit" */
    
  /* pour toutes resolutions M = 1 .. m=2^p */
  for ( S = 1, M = 1, lbb = lroot, rbb = rroot; M != ssi.m;  M *= 2, S *= 4, lbb -= S, rbb -= S )
    {
      /* traitement des collisions pour chaque conflit */
      s = cfhunt( lbb, rbb, bcf, ecf );
      /* si pas de collisions retour */
      if ( !s ) { delete[] bcf; return; };
      /* sinon allocation de l'espace necessaire */
      ncf = new int[ s ];
      /* definition des nouveaux conflits a tester */
      //      std::cout << "cfforward\n";
      cfforward(ncf,bcf,ecf,M);
      /* suppression des anciens conflits */
      delete[] bcf;
      bcf = ncf;
      ecf = ncf + s;
      };
  
  /* traitement du dernier niveau */
  s = cfhunt(lbb,rbb,bcf,ecf);
  ecf = cfsimplify(bcf,ecf);
  ssi.m_bcf = bcf;
  ssi.m_ecf = ecf;
};

/* print a la gnuplot des conflits */
static void cfprint( std::ostream& gp, std::ostream& gpl , int * bcf, int * ecf, int m )
{
  for( int * cf = bcf; cf != ecf; cf += *cf )
    {
      if ( cf[cfbg_] != -1 )
          {
            gp << (cf[cflq_]/m) << " " << (cf[cflq_]%m) << std::endl;
            int * rpq = cf+cfbg_;
            while ( *rpq != -1 && rpq != cf+*cf ) 
              {
                gpl << (*rpq/m) << " " << (*rpq%m) << std::endl;
                rpq++;
              };
          }
    };
};  
void SSIQTS::cfprint( std::ostream& gp, std::ostream& gpl ) 
{ 
  mmx::cfprint(gp, gpl, m_bcf, m_ecf, m ); 
};

SSIQTS::SSIQTS( ParametricSurface * srfa, ParametricSurface * srfb, int n, int degu, int degv )
{
  udeg = degu;
  vdeg = degv; 
  int p = 0;
  m = 1;
  while( n ) { p++; n/=2; m *= 2; };
  smpa = new sample(srfa,m*degu+1,m*degv+1);
  smpb = new sample(srfb,m*degu+1,m*degv+1);  
  int s;
  int deep;
  boxa = alloc(deep,m,s);
  //  std::cout << deep << " " << s << std::endl;
  boxb = alloc(deep,m,s);
  //  std::cout << deep << " " << s << std::endl;
  // switch u interpolant
  // switch v interpolant
  fillboxes( boxa, smpa->m_svals, *this);
  fillboxes( boxb, smpb->m_svals, *this);
  build(boxa,*this);
  build(boxb,*this);
  search(boxa+s-1,boxb+s-1,*this); 
  //  std::cout << "end search\n";
};

SSIQTS::~SSIQTS()
{
  if ( smpa ) delete   smpa;
  if ( smpb ) delete   smpb;
  if ( boxa ) delete[] boxa;
  if ( boxb ) delete[] boxb;
};

} //namespace mmx

#undef ParametricSurface