include/shape/ssi/ssiqts.hpp

Go to the documentation of this file.

#ifndef SYNAPS_SHAPE_SSIQTS_H
#define SYNAPS_SHAPE_SSIQTS_H

# include <shape/parametric_surface.hpp>
# include <shape/fxv.hpp>
# include <shape/ssi/ssi_def.hpp>
# include <iostream>

#define TMPL template<class C, class V>
#define SSIQTS ssiqts<C,V>
//#define ParametricSurface shape::parametric_surface<double>

namespace mmx {

template<class C, class V= shape::default_env>
struct ssiqts
{
  typedef fxv<double,3> vector3;
  typedef double aabb3 [3][2];
  typedef double ipoint [7];
  typedef typename shape::use<shape::ssi_def,C,V>::ParametricSurface  ParametricSurface;

  int udeg;
  int vdeg;
  int m;
  
  struct sample
  {

    double  * m_uvals;
    double  * m_vvals;
    vector3 * m_svals;
    int       m_nrows;
    int       m_ncols;
    inline       vector3& operator[]( int i )       { return m_svals[i]; }
    inline const vector3& operator[]( int i ) const { return m_svals[i]; }
    sample( ParametricSurface * s, int m, int n );
    ~sample();
  };
  
  sample * smpa;
  sample * smpb;
  
  aabb3  * boxa;
  aabb3  * boxb;

  int * m_bcf;
  int * m_ecf;

  void cfprint( std::ostream& gpr, std::ostream& gpl );

  ssiqts( ParametricSurface * srfa,  ParametricSurface * srfb, int n, int degu, int degv );
  ~ssiqts();

  static aabb3 * alloc( int& l, int m, int& s );
  static void fillpatchbox( aabb3& b, const vector3 * a, SSIQTS& ssi );
  static void fillboxes( aabb3 * qta, const vector3 * base, SSIQTS& ssi );
  static bool intersectp( const aabb3 & a, const aabb3 & b );
  static int  cfhunt( aabb3 * lbb, aabb3 * rbb, int * bcf, int * ecf );
  static void build( aabb3 * base, const SSIQTS & ssi );
  static void merge( aabb3 & box, const aabb3& a, const aabb3 & b );
  static void update( aabb3& box, const aabb3& a, const aabb3& b );
  static void search( aabb3 * lroot, aabb3 * rroot, SSIQTS & ssi  );

};

static bool swap;

TMPL
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;
  shape::use<shape::ssi_def,C,V>::sample(s,(double*)m_svals,m,n,m_uvals,m_vvals);
  //  for ( int i = 0; i < m*n; i ++ ) std::cout << m_svals[i]  << std::endl;
};

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

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

TMPL
void SSIQTS::fillpatchbox( typename SSIQTS::aabb3& b, const typename 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 typename SSIQTS::vector3 *  la = a;
  const typename SSIQTS::vector3 * ela = a + (ssi.udeg+1)*ncols;
  const typename 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];
        };
};


TMPL
//static
void SSIQTS::fillboxes( typename SSIQTS::aabb3 * qta, const typename 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 typename SSIQTS::vector3 * lptr  = base;
  const typename SSIQTS::vector3 * cptr;
  const typename 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 );
};

TMPL
//static
bool SSIQTS::intersectp( const typename SSIQTS::aabb3 & a, const typename 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;
};

TMPL
//static
void SSIQTS::merge( typename SSIQTS::aabb3 & box, const typename SSIQTS::aabb3& a, const typename 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]);
    };
};

TMPL
//static
void SSIQTS::update( typename SSIQTS::aabb3& box, const typename SSIQTS::aabb3& a, const typename 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;
    };
};

TMPL
//static
void SSIQTS::build( typename SSIQTS::aabb3 * base, const SSIQTS & ssi )
{
  int N = ssi.m;
  typename 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 */
TMPL
//static
int SSIQTS::cfhunt(
                  typename SSIQTS::aabb3 * lbb, /* ensemble des boites de la surface "gauche" au niveau actuel de subdivision */
                  typename 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  );
};


TMPL
//static
void SSIQTS::search( typename SSIQTS::aabb3 * lroot, typename 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;

  typename SSIQTS::aabb3 *lbb = lroot; /* represente le niveau courant dans le quadtree "gauche" */
  typename 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++;
              };
          }
    };
};

TMPL
void SSIQTS::cfprint( std::ostream& gp, std::ostream& gpl )
{
  mmx::cfprint(gp, gpl, m_bcf, m_ecf, m );
};

TMPL
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";
};

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

} //namespace mmx

# undef TMPL
# undef SSIQTS
# undef ParametricSurface
# endif

---