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

Go to the documentation of this file.

#include <shape/ssi_dsearch.hpp>
#include <shape/ssi_tri_tri.hpp>
#include <shape/ssi_math.hpp>

namespace mmx {
#define SHIFT 3

namespace  shape_ssi {
  
  static const int up_triangle_[] = { 0, 1, 3 };
  static const int dw_triangle_[] = { 2, 3, 1 };

#define __up__   0
#define __down__ 1
    /* return the up triangle of quad q */
#define __up__triangle__(q)         q[0],q[3],q[2]
    /* return the down triangle of quad q */
#define __down__triangle__(q)       q[0],q[1],q[2]
    /* return the parametric plane of up triangle of q */
#define __up__param_triangle__(q)   q[3],q[2],q[0]
    /* return the parametric plane of down triangle of q */
#define __down__param_triangle__(q) q[1],q[0],q[2]
#define __convert_order__(i,point) { point[(i+1)%2] = 1.0-point[(i+1)%2]; }
  /* check for the intersection beetween {UP,DOWN}/{UP,DOWN} triangles */
#define __triangle_triangle_case__(trig0,trig1)                           \
    {                                                                     \
      if ( geom::intersectp_triangles3_isegment                           \
           ( coplanar, seg[0], seg[1],                                    \
             trig0##triangle__(a),                                        \
             trig1##triangle__(b), point3::value_type(1e-12) ))           \
        {                                                                 \
          if ( !coplanar )                                                \
            {                                                             \
              space2prm(/* points in parameter space */                    \
                     seg0[0], seg0[1],                                    \
                     /* corresponding points in R3 */                     \
                      seg[0],  seg[1],                                    \
                     /* parametric plane = base + 2 vectors  */           \
                     trig0##param_triangle__(a)                           \
                     );                                                   \
              space2prm( seg1[0], seg1[1],                                 \
                        seg[0], seg[1],                                   \
                        trig1##param_triangle__(b) );                     \
              /* retrieve the correct offset coordinates in quad */       \
            __convert_order__(trig0,seg0[0]);                             \
            __convert_order__(trig0,seg0[1]);                             \
            __convert_order__(trig1,seg1[0]);                             \
            __convert_order__(trig1,seg1[1]);                             \
            }                                                             \
        else                                                              \
          {\
            seg0[0][0] = 1.0/3.0; seg0[0][1] = 1.0/3.0;                   \
            seg0[1][0] = 1.0/3.0; seg0[1][0] = 1.0/3.0;                   \
            seg1[0][0] = 1.0/3.0; seg1[0][1] = 1.0/3.0;                   \
            seg1[1][0] = 1.0/3.0; seg1[1][1] = 1.0/3.0;                   \
            __convert_order__(trig0,seg0[0]);                             \
            __convert_order__(trig0,seg0[1]);                             \
            __convert_order__(trig1,seg1[0]);                             \
            __convert_order__(trig1,seg1[1]);                             \
          };                                                              \
            {                                                             \
              f->convert(seg0,this,2);                                    \
              s->convert(seg1,this,2);                                    \
              push( seg0[0], seg0[1], seg1[0], seg1[1], seg[0],seg[1] );  \
            }                                                             \
        };                                                                \
    }                                                                     \

  //              f->convert(seg0,this,2);
  //            s->convert(seg1,this,2);
    
  

  void space2prm( vector2 & pa, 
                  vector2 & pb,
                  const vector3& sa, 
                  const vector3& sb,
                  const vector3& base, 
                  const vector3& pu, 
                  const vector3& pv )
  {
  /* T(u,v) = base + u*bu +v*bv
     => 
     spc[0] - base[0] = delta[0]  = / bu[0] bv[0]\  / u \
     spc[1] - base[1] = delta[1]  = | bu[1] bv[1]|  |   |
     spc[2] - base[2] = delta[2]  = \ bu[2] bv[2]/  \ v /
  */
  vector3 bu;
  for ( int i = 0; i < 3; i ++ ) bu[i] = pu[i]-base[i];
  vector3 bv;
  for ( int i = 0; i < 3; i ++ ) bv[i] = pv[i]-base[i];
  double muu, mvv, muv;
  muu = 0;
  for ( int i = 0; i < 3; i ++ ) muu += bu[i]*bu[i];
  mvv = 0;
  for ( int i = 0; i < 3; i ++ ) mvv += bv[i]*bv[i];
  muv = 0; 
  for ( int i = 0; i < 3; i ++ ) muv += bu[i]*bv[i];
  double detm = muu*mvv - muv*muv;
  vector3 delta;
  double x, y;
  for ( int k = 0; k < 3; k ++ ) delta[k] = sa[k]-base[k];
  x = 0;
  for ( int k = 0; k < 3; k ++ ) x += bu[k]*delta[k];
  y = 0; 
  for ( int k = 0; k < 3; k ++ ) y += bv[k]*delta[k];
  pa[0] = (mvv * x - muv * y)/detm;
  pa[1] = (muu * y - muv * x)/detm;
  for ( int k = 0; k < 3; k ++ ) delta[k] = sb[k]-base[k];
  x = 0;
  for ( int k = 0; k < 3; k ++ ) x += bu[k]*delta[k];
  y = 0; 
  for ( int k = 0; k < 3; k ++ ) y += bv[k]*delta[k];
  pb[0] = (mvv * x - muv * y)/detm;
  pb[1] = (muu * y - muv * x)/detm;
};
    
void dsearch::cnfpush( qtree f, qtree s )
{
  point3 a[4],b[4]; //      point3 a[4]; point3 b[4];
  f->fill(a,this);
  s->fill(b,this);
  cnfdata3.push_back( ((double)(1.0/4.0))*(a[0]+a[1]+a[2]+a[3]) );
};

// push an quad / quad conflict
// intersection is computed if any and added to the current conflict set of segments
void dsearch::push( qtree f, qtree s )
{
  if ( std::abs(f->umin-s->umin)+std::abs(f->vmin-s->vmin) < SHIFT ) return;
  conflict_count ++;
  point3 a[4],b[4]; 
  scale_conflict(a,b,f,s);
  {
    point3  seg[2];
    point2 seg0[2], seg1[2];
    bool   coplanar;
    __triangle_triangle_case__(__up__,__up__);
    __triangle_triangle_case__(__up__,__down__);
    __triangle_triangle_case__(__down__,__down__);
    __triangle_triangle_case__(__down__,__up__);
  }
};
/*
  void vector_sub( point3& r, const point3& a, const point3& b )
  {
  for ( unsigned i = 0; i < d; i++ )
  r[i] = a[i]-b[i];
  };
  
  double_t vector_dotprod( const point3& a, const point3& b )
  {
  double_t tmp = 0;
  for ( unsigned i = 0; i < d; i++ )
  tmp += a[i]*b[i];
  return tmp;
  };
  
  void vector_crossprod( point3& r, const point3& a, const point3& b )
    {
    r[2] = a[0]*b[1]-a[1]*b[0];
    r[0] = a[1]*b[2]-a[2]*b[1];
    r[1] = a[2]*b[0]-a[0]*b[2];
    };
    double_t vector_nmax( const point3& a, const point3& b)
    {
    double_t m = std::abs(a[0]-b[0]);
    for ( unsigned i = 1; i < d; i++ )
    {
    double_t c = std::abs(a[i]-b[i]);
    if ( c > m ) m = c;
    };
    return m;
    };
    
    struct FirstOrderException{};
    
    void reachpt( double_t& du, double_t& dv, const point_t& delta, const point_t& Tu, const point_t& Tv )
    {
    number_t v0,v1;
    number_t m00,m11,m01;
    v0  = vector_dotprod( delta, Tu );
    v1  = vector_dotprod( delta, Tv );
    m00 = vector_dotprod( Tu, Tu );
    m11 = vector_dotprod( Tv, Tv );
    m01 = vector_dotprod( Tu, Tv );
    number_t det = m00*m11-m01*m01;
    if ( det < 1e-20 )  throw FirstOrderException(); 
    du = (m11*v0-m01*v1)/det;
    dv = (m00*v1-m01*v0)/det;
    };
    
    bool hosaka_iter( number_t& du, number_t& dv, number_t& ds, number_t& dt, 
    point_t& pA, point_t& pB, 
    const number_t& u, const number_t& v, const number_t& s, const number_t& t )
    {
    
    point_t tAu,tAv,tBu,tBv;

    m_smp->DiffUEval(tAu,u,v); m_smp->DiffVEval(tAv,u,v);
    m_smp->DiffUEval(tBu,s,t); m_smp->DiffVEval(tBv,s,t);
    
    point_t nA,nB,nD;
    
    vector_crossprod( nA, tAu, tAv );
    vector_crossprod( nB, tBu, tBv );      
    vector_crossprod( nD, nA, nB   );
    
    number_t dA,dB,dD;
    
    dA = vector_dotprod( nA, pA );
    dB = vector_dotprod( nB, pB );
    dD = vector_dotprod( nD, pB );
    
    
    number_t d2nD = vector_dotprod( nD, nD ) ;
    number_t d2nA = vector_dotprod( nA, nA ) ;
    number_t d2nB = vector_dotprod( nB, nB ) ;
    
    if ( d2nD < 1e-6*d2nA*d2nB ) throw FirstOrderException();
    
    // plane3
    
    point_t c0,c1,x;
    
    vector_crossprod( c0, nB, nD );
    vector_crossprod( c1, nD, nA );
    
    x[0] = c0[0]*dA+c1[0]*dB+nD[0]*dD;
    x[1] = c0[1]*dA+c1[1]*dB+nD[1]*dD;
    x[2] = c0[2]*dA+c1[2]*dB+nD[2]*dD;
    
    number_t sc = 1.0/d2nD;
    
    x[0] *= sc;
    x[1] *= sc;
    x[2] *= sc;
    
    
    point_t Adelta,Bdelta;
    
    vector_sub(Adelta,x,pA);
    vector_sub(Bdelta,x,pB);
    
    reachpt( du, dv, Adelta, tAu, tAv );
    reachpt( ds, dt, Bdelta, tBu, tBv );
    
    };
    
    
    bool hosaka( point2& l, point2& r )
    {
    point3 pA,pB;
    number u,v,s,t;
    
    const number Sdu(m_smp->du()/3.0);
    const number Sdv(m_smp->dv()/3.0);
    
    u = l[0]*this->m_du+this->Fct->umin();
    v = l[1]*m_dv+this->Fct->vmin();
    
    s = r[0]*this->m_du+this->Fct->umin();
    t = r[1]*m_dv+this->Fct->vmin(); 
    
    this->Fct->Eval(pA,u,v);
    this->Fct->Eval(pB,s,t);
    
    number d = vector_nmax(pA,pB);
    
         #define exiting()\
         {\
         if ( std::abs(du) > Sdu ) return false;\
         if ( std::abs(ds) > Sdu ) return false;\
         if ( std::abs(dv) > Sdv ) return false;\
         if ( std::abs(dt) > Sdv ) return false;\
         l[0] = u;\
         l[1] = v;\
           r[0] = s;\
           r[1] = t;\
          }


      for( int it = 0; it < 8; it++ )
        {
          number_t du,dv,ds,dt;
          try  { hosaka_iter(du,dv,ds,dt,pA,pB,u,v,s,t); }
          catch( ... ) 
            {
              l[0] = u; l[1] = v;
              r[0] = s; r[1] = t;
              return false;
            }

          exiting();

          u += du;
          v += dv;
          s += ds;
          t += dt;
          
          this->Fct->Eval(pA,u,v);
          this->Fct->Eval(pB,s,t);
          
          number_t err = vector_nmax(pA,pB);

          if (  err  <  d    )  d = err; else return false;
          if (  err  <  1e-7 ) 
          {
            using namespace std;
            if ( abs(u-s) < 1e-8 && abs(s-t) < 1e-8 ) return false;
            return true;
          };
        };
      return false;
    };
    */

};
} //namespace mmx