www.pudn.com > AdRBF.rar > AdRbfGenerator.h

#ifndef ADRBFGENERATOR 
#define ADRBFGENERATOR 
 
#include "PointSet.h" 
#include "AxisKdTree.h" 
#include "BasisFunction.h" 
#include "EvaluationTreeD.h" 
#include "PBCG.h" 
#include  
#include  
#include "SVD.h" 
#include "jacobi.h" 
 
class ElementList{ 
public: 
  int index; 
  ElementList* next; 
  double c; 
   
  ~ElementList(){ 
    if(next != NULL) 
      delete next; 
  } 
   
  void add(int i, double w){ 
    if(index == i) 
      c += w; 
    else{ 
      if(next == NULL){ 
        next = new ElementList; 
        next->index = i; 
        next->c = w; 
        next->next = NULL; 
      } 
      else 
        next->add(i, w); 
    } 
  } 
}; 
 
class GraphNode{ 
public: 
  int index; 
  GraphNode* next; 
   
  GraphNode(){ 
    index = -1; 
    next = NULL; 
  } 
   
  ~GraphNode(){ 
    if(next != NULL) 
      delete next; 
  } 
   
  void add(int i){ 
    if(index < 0){ 
      index = i; 
    } 
    else if(index == i) 
      return; 
    else{ 
      if(next == NULL){ 
        next = new GraphNode; 
        next->index = i; 
        next->next = NULL; 
      } 
      else 
        next->add(i); 
    } 
  } 
}; 
 
class AdRbfGenerator{ 
public: 
  PointSet* ps; 
  AxisKdTree* tree; 
  int canN; 
  float overlapT; 
  float e0; 
  int bfN; 
  BasisFunction** bfs; 
  float dia; 
   
  AdRbfGenerator(PointSet* ps){ 
    this->ps = ps; 
    canN = 15; 
    overlapT = 1.5f; 
     
    float max[3], min[3]; 
    ps->getBound(min, max, 1.0); 
    float sx = max[0] - min[0]; 
    float sy = max[1] - min[1]; 
    float sz = max[2] - min[2]; 
    dia = (float)sqrt(sx*sx + sy*sy + sz*sz); 
     
    e0 = 0.0001f; 
  } 
   
  void generate(){ 
    //quadric approx. 
    generateBfsQB(); 
     
    //linear approx. 
    //generateBfsPB(); 
     
    //RBF fitting 
    globalFit(); 
     
    //Oriented normals are unknown case. 
    //createOrientation(); 
     
    //printError(); 
  } 
   
  void globalFit(){ 
    int i,j,k; 
     
    int M = bfN; 
    float max[3], min[3]; 
    ps->getBound(min, max, 1.2f); 
    EvaluationTreeD* func = new EvaluationTreeD(min, max); 
    func->addBFS(bfs, M); 
    func->out = 0; 
     
    int N = ps->_pointN; 
    float (*full_point)[3] = ps->_point; 
    float *value = ps->_value; 
    double L2_before=0, L2_after=0; 
    double Lmax_before=0, Lmax_after=0; 
     
    ElementList** element_list = new ElementList*[M]; 
    double* b = new double[M+1]; 
     
    for(i=0; iindex = i; 
      element_list[i]->c = 0; 
      element_list[i]->next = NULL; 
      b[i+1] = 0; 
    } 
     
    int listN, *list; 
    double* weight; 
    float total_value = 0; 
    for(i=0; ivalueAndGradient1(g, p[0], p[1], p[2])*value[i]; 
      double g2 = g[0]*g[0]+g[1]*g[1]+g[2]*g[2]; 
      if((float)g2 != 0){ 
        total_value += value[i]; 
        L2_before += v*v/g2; 
         
        double e = fabs(v)/sqrt(g2); 
        if(Lmax_before < e) 
          Lmax_before = e; 
      } 
       
      func->collectIndexInSupport(list, weight, listN, p); 
      for(j=0; jadd(i1, w1*w1); 
         
        for(k=j+1; kadd(i2, w); 
          element_list[i2]->add(i1, w); 
        } 
        b[i1+1] += v*w1; 
      } 
      if(i%1000 == 0){ 
        printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b%d points are processed.", i); 
        //cout << i << "points are processed." << endl; 
      } 
    } 
    cout << endl; 
     
    int total = 0; 
    for(i=0; inext) 
        total++; 
    cout << "Non-zero element in the matrix; " << total << endl; 
     
    PBCG solver; 
    double *sa = solver.sa = new double[total+2]; 
    unsigned long *ija = solver.ija = new unsigned long[total+2]; 
    double* sol = new double[bfN+1]; 
    ija[1]=bfN+2; 
    int kk=bfN+1; 
     
    int* i_tmp = new int[M-1]; 
    double* w_tmp = new double[M-1]; 
    for(i=0; inext){ 
        if(e->index != i){ 
          i_tmp[m] = e->index; 
          w_tmp[m] = e->c; 
          m++; 
        } 
        else{ 
          //diagonal element 
          sa[i+1] = e->c; 
        } 
      } 
       
      //sort 
      delete element_list[i]; 
      for(j=0; j i_tmp[k]){ 
            int tmp = i_tmp[j]; 
            i_tmp[j] = i_tmp[k]; 
            i_tmp[k] = tmp; 
             
            double tmp2 = w_tmp[j]; 
            w_tmp[j] = w_tmp[k]; 
            w_tmp[k] = tmp2; 
          } 
        } 
      } 
       
      for(j=0; jc0 += (float)sol[i+1]; 
    } 
    delete[] sol; 
     
    for(i=0; ivalueAndGradient1(g, p[0], p[1], p[2])*value[i]; 
      double g2 = g[0]*g[0]+g[1]*g[1]+g[2]*g[2]; 
      if((float)g2 != 0){ 
        L2_after += v*v/g2; 
         
        double e = fabs(v)/sqrt(g2); 
        if(Lmax_after < e) 
          Lmax_after = e; 
      } 
    } 
    cout << "L2 before" << sqrt(L2_before/total_value)/dia << endl; 
    cout << "L2 after " << sqrt(L2_after/total_value)/dia << endl; 
     
    cout << "Lmax before" << Lmax_before/dia << endl; 
    cout << "Lmax after " << Lmax_after/dia << endl; 
  } 
   
  void generateBfsQB(){ 
    int N = ps->_pointN; 
    float (*point)[3] = ps->_point; 
    float (*normal)[3] = ps->_normal; 
    float *conf = ps->_value; 
     
    tree = new AxisKdTree(ps); 
     
    float* overlap = new float[N]; 
    int* active_table = new int[N]; 
    int activeN = 0; 
    int tableN = 0; 
     
    int i,j; 
    for(i=0; i 0.2){ 
        overlap[i] = 0; 
        active_table[activeN++] = i; 
      } 
    } 
    cout << activeN << " points are candidates of centers" << endl; 
    N = tableN = activeN; 
     
    float w0 = (float)BasisFunction::weight(0, 1); 
     
    bfN = 0; 
    BFlist* bf_list = new BFlist(); 
    bf_list->next = NULL; 
     
    int* can = new int[canN]; 
    long idum = 1; 
     
    //For counter to stdout 
    int pre_activeN = activeN; 
    int percent = 0; 
     
    float e0r = e0*dia; 
     
    while(activeN > canN){ 
      for(j=0; j= overlapT) 
          j--; 
      } 
       
      //search minimun from multiple-choice 
      int index = can[0]; 
      for(j=1; j overlap[can[j]]) 
          index = can[j]; 
      } 
       
      overlap[index] = overlapT; 
      activeN--; 
       
      float *center = point[index]; 
       
      float scaleL = 50*e0r; 
      float scaleS = e0r; 
       
      BasisFunction* bf = new BasisFunction; 
       
      float scale, error; 
      error = localFitQ(center, scaleL, bf); 
      //error = localFitQwithCV(center, scaleL, bf); 
      while(error < e0r){ 
        scaleS = scaleL; 
        scaleL *= 2; 
        error = localFitQ(center, scaleL, bf); 
        //error = localFitQwithCV(center, scaleL, bf); 
      } 
      for(j=0; j<10; j++){ 
        scale = 0.5f*(scaleS + scaleL); 
         
        error = localFitQ(center, scale, bf); 
        //error = localFitQwithCV(center, scale, bf); 
         
        if(error > e0r) 
          scaleL = scale; 
        else{ 
          scaleS = scale; 
        } 
      } 
      if(error == 0){ 
        scale = bf->support = scaleL; 
      } 
       
      //updtae overlap 
      float R = scale; 
      int *list, listN; 
      tree->collectPointIndexInSphere(list, listN, center, scale); 
      for(j=0; j d){ 
          float w = (float)BasisFunction::weight(d, R)/w0; 
          if(overlap[in] < overlapT){ 
            overlap[in] += w; 
            if(overlap[in] >= overlapT) 
              activeN--; 
          } 
          else 
            overlap[in] += w; 
        } 
      } 
       
      //Add basis function to the list 
      bf->level = 1; 
      BFlist* l = new BFlist; 
      l->next = bf_list; 
      l->bf = bf; 
      bf_list = l; 
      bfN++; 
       
      if(activeN < tableN/2){ //resize table 
        int count = 0; 
        for(j=0; j percent) 
        printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b%d %%", ++percent); 
      //cout << ++percent << " %" << endl; 
      pre_activeN = activeN; 
    } 
    cout << endl; 
    delete tree; 
    delete[] overlap; 
    delete[] active_table; 
     
    bfs = new BasisFunction*[bfN]; 
    int count = 0; 
    for(BFlist* l=bf_list; l->next!=NULL; l=l->next){ 
      bfs[count] = l->bf; 
      bfs[count]->index = count; 
      count++; 
    } 
    //delete bf_list; 
    cout << bfN << " basis functions" << endl; 
  } 
   
  float localFitQ(float center[3], float scale, BasisFunction* bf){ 
    float (*point)[3] = ps->_point; 
    float (*normal)[3] = ps->_normal; 
     
    bf->centerX = center[0]; 
    bf->centerY = center[1]; 
    bf->centerZ = center[2]; 
     
    bf->support = scale; 
     
    int *list, listN; 
    tree->collectPointIndexInSphere(list, listN, center, scale); 
     
    if(listN < 10) 
      return 0; 
     
    float n[3], t1[3], t2[3]; 
    computeNormal(n, list, listN, bf); 
    computeTangent(t1, t2, n); 
     
    float **A = new float*[7]; 
    float *b = new float[7]; 
	int i; 
    for(i=1; i<7; i++){ 
      A[i] = new float[7]; 
      A[i][1] = A[i][2] = A[i][3] = A[i][4] = 
      A[i][5] = A[i][6] = b[i] = 0; 
    } 
    for(i=0; i wmax) wmax=(float)fabs(w[k]); 
    /* 
      if(wmax < 0.000000000001f){ 
        bf_N--; 
        return; 
      }*/ 
     
    float wmin=wmax*0.0000001f; 
    for (k=1;k<7;k++){ 
      if (fabs(w[k]) < wmin)  
        w[k]=0.0; 
    } 
     
    SVD::svbksb(A, w, v, 6, 6, b, x); 
     
    for(i=1; i<7; i++) 
      delete[] A[i]; 
    delete[] A; 
    for(i=1; i<7; i++) 
      delete[] v[i]; 
    delete[] v; 
    delete[] b; 
     
    float cuu = x[1]; 
    float cuv = x[2]; 
    float cvv = x[3]; 
    float cu = x[4]; 
    float cv = x[5]; 
    float c0 = x[6]; 
     
    bf->cXX = -(cuu*t1[0]*t1[0] + cvv*t2[0]*t2[0] + cuv*t1[0]*t2[0]); 
    bf->cYY = -(cuu*t1[1]*t1[1] + cvv*t2[1]*t2[1] + cuv*t1[1]*t2[1]); 
    bf->cZZ = -(cuu*t1[2]*t1[2] + cvv*t2[2]*t2[2] + cuv*t1[2]*t2[2]); 
     
    bf->cXY = -(2.0f*(cuu*t1[0]*t1[1] + cvv*t2[0]*t2[1]) + cuv*(t1[0]*t2[1] + t1[1]*t2[0])); 
    bf->cYZ = -(2.0f*(cuu*t1[1]*t1[2] + cvv*t2[1]*t2[2]) + cuv*(t1[1]*t2[2] + t1[2]*t2[1])); 
    bf->cZX = -(2.0f*(cuu*t1[2]*t1[0] + cvv*t2[2]*t2[0]) + cuv*(t1[2]*t2[0] + t1[0]*t2[2])); 
     
    bf->cX = n[0] - cu*t1[0] - cv*t2[0]; 
    bf->cY = n[1] - cu*t1[1] - cv*t2[1]; 
    bf->cZ = n[2] - cu*t1[2] - cv*t2[2]; 
     
    bf->c0 = -c0; 
     
    //compute L2 error 
    double error = 0; 
    double totalW = 0; 
    for(i=0; i_point; 
    //float (*normal)[3] = ps->_normal; 
    float *conf = ps->_value; 
     
    bf->centerX = center[0]; 
    bf->centerY = center[1]; 
    bf->centerZ = center[2]; 
     
    bf->support = scale; 
     
    int *list, listN; 
    tree->collectPointIndexInSphere(list, listN, center, scale); 
     
    float sumC = 0; 
    for(i=0; i w[3]) 
      mini = 3; 
     
    n[0] = v[1][mini]; 
    n[1] = v[2][mini]; 
    n[2] = v[3][mini]; 
     
    //orientation checking 
    /* 
    float dotS = 0; 
    for(i=0; i wmax) wmax=(float)fabs(w[k]); 
    /* 
      if(wmax < 0.000000000001f){ 
        bf_N--; 
        return; 
      }*/ 
     
    float wmin=wmax*0.0000001f; 
    for (k=1;k<7;k++){ 
      if (fabs(w[k]) < wmin)  
        w[k]=0.0; 
    } 
     
    SVD::svbksb(A, w, v, 6, 6, b, x); 
     
    for(i=1; i<7; i++) 
      delete[] A[i]; 
    delete[] A; 
    for(i=1; i<7; i++) 
      delete[] v[i]; 
    delete[] v; 
    delete[] b; 
     
    float cuu = x[1]; 
    float cuv = x[2]; 
    float cvv = x[3]; 
    float cu = x[4]; 
    float cv = x[5]; 
    float c0 = x[6]; 
     
    bf->cXX = -(cuu*t1[0]*t1[0] + cvv*t2[0]*t2[0] + cuv*t1[0]*t2[0]); 
    bf->cYY = -(cuu*t1[1]*t1[1] + cvv*t2[1]*t2[1] + cuv*t1[1]*t2[1]); 
    bf->cZZ = -(cuu*t1[2]*t1[2] + cvv*t2[2]*t2[2] + cuv*t1[2]*t2[2]); 
     
    bf->cXY = -(2.0f*(cuu*t1[0]*t1[1] + cvv*t2[0]*t2[1]) + cuv*(t1[0]*t2[1] + t1[1]*t2[0])); 
    bf->cYZ = -(2.0f*(cuu*t1[1]*t1[2] + cvv*t2[1]*t2[2]) + cuv*(t1[1]*t2[2] + t1[2]*t2[1])); 
    bf->cZX = -(2.0f*(cuu*t1[2]*t1[0] + cvv*t2[2]*t2[0]) + cuv*(t1[2]*t2[0] + t1[0]*t2[2])); 
     
    bf->cX = n[0] - cu*t1[0] - cv*t2[0]; 
    bf->cY = n[1] - cu*t1[1] - cv*t2[1]; 
    bf->cZ = n[2] - cu*t1[2] - cv*t2[2]; 
     
    bf->c0 = -c0; 
     
    //compute L2 error 
    double error = 0; 
    double totalW = 0; 
    for(i=0; i error) 
        //error = e; 
    } 
    return sqrt(error/listN); //dia; 
    //cout << scale << "::" << error << endl; 
    //return error; 
  } 
 
  float localFitPwithCV(float center[3], float scale, BasisFunction* bf){ 
    int i; 
    float (*point)[3] = ps->_point; 
    float (*normal)[3] = ps->_normal; 
     
    bf->centerX = center[0]; 
    bf->centerY = center[1]; 
    bf->centerZ = center[2]; 
     
    bf->support = scale; 
     
    int *list, listN; 
    tree->collectPointIndexInSphere(list, listN, center, scale); 
     
    if(listN < 5) 
      return 0; 
     
    float n[3], t1[3], t2[3]; 
     
    float **A = new float*[4]; 
    float *b = new float[4]; 
    for(i=1; i<4; i++){ 
      A[i] = new float[4]; 
      A[i][1] = A[i][2] = A[i][3] = b[i] = 0; 
    } 
     
    float w[4], x[4]; 
    float **v = new float*[4]; 
    for(i=1; i<4; i++) 
      v[i] = new float[4]; 
     
    //Computation of normal using CV method 
    for(i=0; i w[3]) 
      mini = 3; 
     
    n[0] = v[1][mini]; 
    n[1] = v[2][mini]; 
    n[2] = v[3][mini]; 
     
    //orientation checking 
    /* 
    float dotS = 0; 
    for(i=0; i wmax) wmax=(float)fabs(w[k]); 
    /* 
      if(wmax < 0.000000000001f){ 
        bf_N--; 
        return; 
      }*/ 
     
    float wmin=wmax*0.0000001f; 
    for (k=1;k<4;k++){ 
      if (fabs(w[k]) < wmin)  
        w[k]=0.0; 
    } 
     
    SVD::svbksb(A, w, v, 3, 3, b, x); 
     
    for(i=1; i<4; i++) 
      delete[] A[i]; 
    delete[] A; 
    for(i=1; i<4; i++) 
      delete[] v[i]; 
    delete[] v; 
    delete[] b; 
     
    float cuu = 0; 
    float cuv = 0; 
    float cvv = 0; 
    float cu = x[1]; 
    float cv = x[2]; 
    float c0 = x[3]; 
     
    bf->cXX = 0; 
    bf->cYY = 0; 
    bf->cZZ = 0; 
     
    bf->cXY = 0; 
    bf->cYZ = 0; 
    bf->cZX = 0; 
     
    bf->cX = n[0] - cu*t1[0] - cv*t2[0]; 
    bf->cY = n[1] - cu*t1[1] - cv*t2[1]; 
    bf->cZ = n[2] - cu*t1[2] - cv*t2[2]; 
     
    bf->c0 = -c0; 
     
    //compute L2 error 
    double error = 0; 
    double totalW = 0; 
    for(i=0; i_pointN; 
    float (*point)[3] = ps->_point; 
    float (*normal)[3] = ps->_normal; 
     
    tree = new AxisKdTree(ps); 
     
    float* overlap = new float[N]; 
    int* active_table = new int[N]; 
    int activeN = N; 
    int tableN = N; 
     
    int i,j; 
    for(i=0; inext = NULL; 
     
    int* can = new int[canN]; 
    long idum = 1; 
     
    //For counter to stdout 
    int pre_activeN = activeN; 
    int percent = 0; 
     
    while(activeN > canN){ 
      for(j=0; j= overlapT) 
          j--; 
      } 
       
      //search minimun from multiple-choice 
      int index = can[0]; 
      for(j=1; j overlap[can[j]]) 
          //if(overlap[index]*value[index] > overlap[can[j]]*value[can[j]]) 
          index = can[j]; 
      } 
       
      overlap[index] = overlapT; 
      activeN--; 
       
      float *center = point[index]; 
       
      float scaleL = 50*e0*dia; 
      float scaleS = e0*dia; 
      BasisFunction* bf = new BasisFunction; 
      float scale, error; 
      error = localFitPwithCV(center, scaleL, bf); 
      while(error < e0){ 
        scaleS = scaleL; 
        scaleL *= 2; 
        error = localFitPwithCV(center, scaleL, bf); 
      } 
      for(j=0; j<10; j++){ 
        scale = 0.5f*(scaleS + scaleL); 
        error = localFitPwithCV(center, scale, bf); 
        //cout << j << ", " << scale << ", " << error << endl; 
        if(error > e0) 
          scaleL = scale; 
        else{ 
          scaleS = scale; 
        } 
      } 
	  if(error == 0){ 
        bf->support = scaleL; 
      } 
       
      //updtae overlap 
      float R = scale; 
      int *list, listN; 
      tree->collectPointIndexInSphere(list, listN, center, scale); 
      for(j=0; j d){ 
          float w = (float)BasisFunction::weight(d, R)/w0; 
          if(overlap[in] < overlapT){ 
            overlap[in] += w; 
            if(overlap[in] >= overlapT) 
              activeN--; 
          } 
          else 
            overlap[in] += w; 
        } 
      } 
       
      //Add basis function to the list 
      bf->level = 1; 
      BFlist* l = new BFlist; 
      l->next = bf_list; 
      l->bf = bf; 
      bf_list = l; 
      bfN++; 
       
      if(activeN < tableN/2){ //resize table 
        int count = 0; 
        for(j=0; j percent) 
        printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b%d %%", ++percent); 
      //cout << ++percent << " %" << endl; 
      pre_activeN = activeN; 
    } 
    cout << endl; 
    delete tree; 
    delete[] overlap; 
    delete[] active_table; 
     
    bfs = new BasisFunction*[bfN]; 
    int count = 0; 
    for(BFlist* l=bf_list; l->next!=NULL; l=l->next){ 
      bfs[count] = l->bf; 
      bfs[count]->index = count; 
      count++; 
    } 
    //delete bf_list; 
    cout << bfN << " basis functions" << endl; 
  } 
   
  float localFitP(float center[3], float scale, BasisFunction* bf){ 
    float (*point)[3] = ps->_point; 
    float (*normal)[3] = ps->_normal; 
     
    bf->centerX = center[0]; 
    bf->centerY = center[1]; 
    bf->centerZ = center[2]; 
     
    bf->support = scale; 
     
    int *list, listN; 
    tree->collectPointIndexInSphere(list, listN, center, scale); 
     
    if(listN < 6) 
      return 0; 
     
    float n[3], t1[3], t2[3]; 
    computeNormal(n, list, listN, bf); 
    computeTangent(t1, t2, n); 
     
    float **A = new float*[4]; 
    float *b = new float[4]; 
	int i; 
    for(i=1; i<4; i++){ 
      A[i] = new float[4]; 
      A[i][1] = A[i][2] = A[i][3] = b[i] = 0; 
    } 
    for(i=0; i wmax) wmax=(float)fabs(w[k]); 
    /* 
      if(wmax < 0.000000000001f){ 
        bf_N--; 
        return; 
      }*/ 
     
    float wmin=wmax*0.0000001f; 
    for (k=1;k<4;k++){ 
      if (fabs(w[k]) < wmin)  
        w[k]=0.0; 
    } 
     
    SVD::svbksb(A, w, v, 3, 3, b, x); 
     
    for(i=1; i<4; i++) 
      delete[] A[i]; 
    delete[] A; 
    for(i=1; i<4; i++) 
      delete[] v[i]; 
    delete[] v; 
    delete[] b; 
     
    float cu = x[1]; 
    float cv = x[2]; 
    float c0 = x[3]; 
     
    bf->cXX = 0; 
    bf->cYY = 0; 
    bf->cZZ = 0; 
     
    bf->cXY = 0; 
    bf->cYZ = 0; 
    bf->cZX = 0; 
     
    bf->cX = n[0] - cu*t1[0] - cv*t2[0]; 
    bf->cY = n[1] - cu*t1[1] - cv*t2[1]; 
    bf->cZ = n[2] - cu*t1[2] - cv*t2[2]; 
     
    bf->c0 = -c0; 
     
    //compute L2 error 
    double error = 0; 
    double totalW = 0; 
    for(i=0; igetBound(min, max, 1.2f); 
    EvaluationTreeD* func = new EvaluationTreeD(min, max); 
    func->addBFS(bfs, M); 
    func->out = 0; 
     
    int N = ps->_pointN; 
    float (*full_point)[3] = ps->_point; 
     
    GraphNode** nodes = new GraphNode*[M]; 
    for(i=0; icollectIndexInSupport(list, weight, listN, p); 
      for(j=0; jadd(i2); 
          nodes[i2]->add(i1); 
        } 
      } 
    } 
     
    delete func; 
     
    cout << "A conectivity graph is created." << endl; 
    /* 
    GraphNode** tree = new GraphNode*[M]; 
    for(i=0; icenterZ < bfs[i]->centerZ){ 
        bf = bfs[i]; 
        update = i; 
      } 
    } 
    float g[3]; 
    bf->gradientP(g, bf->centerX, bf->centerY, bf->centerZ); 
    if(g[2] > 0){ 
      bf->cXX = -bf->cXX; 
      bf->cYY = -bf->cYY; 
      bf->cZZ = -bf->cZZ; 
      bf->cXY = -bf->cXY; 
      bf->cYZ = -bf->cYZ; 
      bf->cZX = -bf->cZX; 
      bf->cX = -bf->cX; 
      bf->cY = -bf->cY; 
      bf->cZ = -bf->cZ; 
      bf->c0 = -bf->c0; 
    } 
    decide[update] = true; 
     
     
    for(i=1; inext){ 
        int j = c->index; 
        if(decide[j]) 
          continue; 
         
        BasisFunction* bfj = bfs[j]; 
         
        float w1 = bfj->support/(bfi->support + bfj->support); 
        float w2 = bfi->support/(bfi->support + bfj->support); 
        float x = w1*bfi->centerX + w2*bfj->centerX; 
        float y = w1*bfi->centerY + w2*bfj->centerY; 
        float z = w1*bfi->centerZ + w2*bfj->centerZ; 
        float n1[3], n2[3]; 
        bfi->gradientP(n1, x, y, z); 
        bfj->gradientP(n2, x, y, z); 
        normalize(n1); 
        normalize(n2); 
        float dot = n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]; 
        float d = (1.0f - fabs(dot)); 
         
        /* 
        float n1[3], n2[3], v[3]; 
        bfi->gradientP(n1, bfi->centerX, bfi->centerY, bfi->centerZ); 
        bfj->gradientP(n2, bfj->centerX, bfj->centerY, bfj->centerZ); 
        v[0] = bfj->centerX - bfi->centerX; 
        v[1] = bfj->centerY - bfi->centerY; 
        v[2] = bfj->centerZ - bfi->centerZ; 
         
        normalize(n1); 
        normalize(n2); 
        normalize(v); 
           
		float d = (1.0f - fabs(n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2])); 
          */ 
        /* 
        float c1[2], c2[2]; 
        c1[0] = n1[1]*v[2] - n1[2]*v[1]; 
        c1[1] = n1[2]*v[0] - n1[0]*v[2]; 
        c1[2] = n1[0]*v[1] - n1[1]*v[0]; 
        c2[0] = n2[1]*v[2] - n2[2]*v[1]; 
        c2[1] = n2[2]*v[0] - n2[0]*v[2]; 
        c2[2] = n2[0]*v[1] - n2[1]*v[0]; 
		//normalize(c1); 
		//normalize(c2); 
        float d = (1.0f - 0.5f*(length(c1)+length(c2))); 
		//float d = (1.0f - fabs(c1[0]*c2[0] + c1[1]*c2[1] + c1[2]*c2[2])); 
		*/ 
        /* 
        float dot = (c1[0]*c2[0] + c1[1]*c2[1] + c1[2]*c2[2]); 
        float d = (n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                  /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2])); 
        if(d > 1.0f) 
          d = 1.0f; 
        else if(d < -1.0f) 
          d = -1.0f; 
        if(dot > 0){ 
          d = acos(d); 
        } 
        else{ 
          d = acos(-d); 
        }*/ 
        /* 
        float n1[3], n2[3]; 
        bfi->gradientP(n1, bfi->centerX, bfi->centerY, bfi->centerZ); 
        bfj->gradientP(n2, bfi->centerX, bfi->centerY, bfi->centerZ); 
        float dot1 = (float)(fabs(n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                             /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
        bfi->gradientP(n1, bfj->centerX, bfj->centerY, bfj->centerZ); 
        bfj->gradientP(n2, bfj->centerX, bfj->centerY, bfj->centerZ); 
        float dot2 = (float)(fabs(n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                             /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
         */ 
        /* 
        bfi->gradientP(n1, 0.5f*(bfi->centerX + bfj->centerX), 
                           0.5f*(bfi->centerY + bfj->centerY), 
                           0.5f*(bfi->centerZ + bfj->centerZ)); 
        bfj->gradientP(n2, 0.5f*(bfi->centerX + bfj->centerX), 
                           0.5f*(bfi->centerY + bfj->centerY), 
                           0.5f*(bfi->centerZ + bfj->centerZ)); 
        float dot3 = (float)(fabs(n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                             /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
         
        float d = (1.0f - 0.25f*(dot1 + 2.0f*dot3 + dot2));*/ 
         
        //float d = (1.0f - 0.5f*(dot1+dot2)); 
         
        /* 
        float dot = bfi->normalX*bfj->normalX 
                  + bfi->normalY*bfj->normalY + bfi->normalZ*bfj->normalZ; 
        float d = (1.0f - fabs(dot));*/ 
        /* 
        float vx = bfi->centerX - bfj->centerX; 
        float vy = bfi->centerY - bfj->centerY; 
        float vz = bfi->centerZ - bfj->centerZ; 
        d = d*(float)sqrt(vx*vx + vy*vy + vz*vz);*/ 
         
        if(cost[j] > d){ 
          cost[j] = d; 
          label[j] = update; 
          if(index[j] < 0){ 
            //insert into heap 
            heap[++last_heap] = j; 
            index[j] = last_heap; 
            upheap(cost, last_heap, last_heap, heap, index); 
          } 
          else{ 
            //update heap 
            if(index[j] != 1 && cost[j] < cost[heap[index[j]/2]]) 
              upheap(cost, last_heap, index[j], heap, index); 
            else 
              downheap(cost, last_heap, index[j], heap, index); 
          } 
        } 
      } 
      //remove top at heap 
      int min = heap[1]; 
      index[min] = -1; 
      heap[1] = heap[last_heap--]; 
	  if(last_heap < 0){ 
		cout << "Un-connected components may exist." << endl; 
	    break; 
	  } 
      index[heap[1]] = 1; 
      downheap(cost, last_heap, 1, heap, index); 
       
       
      /* 
      if(cost[min] > 0.1){ 
        cout << cost[min] << ":" << i << endl; 
      }*/ 
       
      update = min; 
      decide[update] = true; 
      cost[update] = 0; 
      int pair = label[update]; 
      bfi = bfs[update]; 
      BasisFunction* bfj = bfs[pair]; 
       
      float w1 = bfj->support/(bfi->support + bfj->support); 
      float w2 = bfi->support/(bfi->support + bfj->support); 
      float x = w1*bfi->centerX + w2*bfj->centerX; 
      float y = w1*bfi->centerY + w2*bfj->centerY; 
      float z = w1*bfi->centerZ + w2*bfj->centerZ; 
      float n1[3], n2[3]; 
      bfi->gradientP(n1, x, y, z); 
      bfj->gradientP(n2, x, y, z); 
      float dot = n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]; 
       
       
      /* 
      float n1[3], n2[3], v[3]; 
      bfi->gradientP(n1, bfi->centerX, bfi->centerY, bfi->centerZ); 
      bfj->gradientP(n2, bfj->centerX, bfj->centerY, bfj->centerZ); 
      v[0] = bfj->centerX - bfi->centerX; 
      v[1] = bfj->centerY - bfi->centerY; 
      v[2] = bfj->centerZ - bfi->centerZ; 
         
	  float dot = n1[0]* n2[0] + n1[1]*n2[1] + n1[2]*n2[2]; 
       
      float c1[2], c2[2]; 
      c1[0] = n1[1]*v[2] - n1[2]*v[1]; 
      c1[1] = n1[2]*v[0] - n1[0]*v[2]; 
      c1[2] = n1[0]*v[1] - n1[1]*v[0]; 
      c2[0] = n2[1]*v[2] - n2[2]*v[1]; 
      c2[1] = n2[2]*v[0] - n2[0]*v[2]; 
      c2[2] = n2[0]*v[1] - n2[1]*v[0]; 
      float dot = (c1[0]*c2[0] + c1[1]*c2[1] + c1[2]*c2[2]);*/ 
      /* 
      bfi->gradientP(n1, bfi->centerX, bfi->centerY, bfi->centerZ); 
      bfj->gradientP(n2, bfi->centerX, bfi->centerY, bfi->centerZ); 
      float dot1 = (float)((n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                           /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
       
      bfi->gradientP(n1, bfj->centerX, bfj->centerY, bfj->centerZ); 
      bfj->gradientP(n2, bfj->centerX, bfj->centerY, bfj->centerZ); 
      float dot2 = (float)((n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                           /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
       */ 
      /* 
      bfi->gradientP(n1, 0.5f*(bfi->centerX + bfj->centerX), 
                     0.5f*(bfi->centerY + bfj->centerY), 
                     0.5f*(bfi->centerZ + bfj->centerZ)); 
      bfj->gradientP(n2, 0.5f*(bfi->centerX + bfj->centerX), 
                     0.5f*(bfi->centerY + bfj->centerY), 
                     0.5f*(bfi->centerZ + bfj->centerZ)); 
      float dot3 = (float)(fabs(n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]) 
                           /(sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]) * sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]))); 
       
      float dot = dot1 + 2.0f*dot3 + dot2;*/ 
      /* 
      float dot = dot1 + dot2; 
	  if(fabs(dot) < 0.1f) 
		  cout << "Sharp feature or very high curvature region is found, maybe mistake: " << dot << endl; 
        */ 
      //float dot = bfi->normalX*bfj->normalX + bfi->normalY*bfj->normalY + bfi->normalZ*bfj->normalZ; 
      if(dot  < 0){		 
        bfi->cXX = -bfi->cXX; 
        bfi->cYY = -bfi->cYY; 
        bfi->cZZ = -bfi->cZZ; 
        bfi->cXY = -bfi->cXY; 
        bfi->cYZ = -bfi->cYZ; 
        bfi->cZX = -bfi->cZX; 
        bfi->cX = -bfi->cX; 
        bfi->cY = -bfi->cY; 
        bfi->cZ = -bfi->cZ; 
        bfi->c0 = -bfi->c0; 
      } 
       
      /* 
      tree[update]->add(pair); 
      tree[pair]->add(update);*/ 
    } 
     
    delete[] decide; 
    delete[] cost; 
    delete[] label;  
     
    delete[] heap; 
    delete[] index; 
     
    for(i=0; inext) 
        degree++; 
      if(maxD < degree) 
        maxD = degree; 
      if(minD > degree) 
        minD = degree; 
      total += degree; 
    } 
    cout << (float)total/M << ":" << maxD << ":" << minD << endl;*/ 
  } 
   
  inline void normalize(float n[3]){ 
    float len = (float)(sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2])); 
    if(len != 0){ 
      len = 1.0f/len; 
      n[0] *= len; 
      n[1] *= len; 
      n[2] *= len; 
    } 
  } 
   
  inline float length(float v[3]){ 
    return (float)(sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2])); 
  } 
   
  inline void upheap(float *a, int N, int k, int *p, int *q){ 
    int v; 
    v = p[k]; 
    while(k > 1 && a[p[k/2]] >= a[v]){ 
      p[k] = p[k/2]; q[p[k/2]] = k; k = k/2; 
    } 
    p[k] = v; q[v] = k; 
  } 
 
  inline void downheap(float *a, int N, int k, int *p, int *q){ 
    int j, v; 
    v = p[k]; 
    while(k <= N/2){ 
      j = k+k; 
      if(j < N && a[p[j]] > a[p[j+1]]) j++; 
      if(a[v] <= a[p[j]]) break; 
      p[k] = p[j]; q[p[j]] = k; k = j; 
    } 
    p[k] = v; q[v] = k; 
  } 
   
  inline bool computeNormal(float n[3], int* list, int listN, BasisFunction* bf){ 
    n[0] = n[1] = n[2] = 0; 
    float (*normal)[3] = ps->_normal; 
    float (*point)[3] = ps->_point; 
    for(int i=0; iweight(p[0], p[1], p[2]); 
      //if(w < 0.99) 
      // continue; 
      n[0] += w*m[0]; 
      n[1] += w*m[1]; 
      n[2] += w*m[2]; 
    } 
     
    double len = sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]); 
    if((float)len == 0) 
      return false; 
    n[0] = (float)(n[0]/len); 
    n[1] = (float)(n[1]/len); 
    n[2] = (float)(n[2]/len); 
    return true; 
  } 
   
  inline void computeTangent(float t1[3], float t2[3], float n[3]){ 
    if(fabs(n[0]) < fabs(n[1])){ 
      double l = sqrt(n[1]*n[1] + n[2]*n[2]); 
      t1[0] = 0; 
      t1[1] = -(float)(n[2]/l); 
      t1[2] = (float)(n[1]/l); 
    } 
    else{ 
      double l = sqrt(n[0]*n[0] + n[2]*n[2]); 
      t1[0] = (float)(n[2]/l); 
      t1[1] = 0; 
      t1[2] = -(float)(n[0]/l); 
    } 
    t2[0] = n[1]*t1[2] - n[2]*t1[1]; 
    t2[1] = n[2]*t1[0] - n[0]*t1[2]; 
    t2[2] = n[0]*t1[1] - n[1]*t1[0]; 
  } 
   
  //From Numerical Recipes in C 
  #define IA 16807 
  #define IM 2147483647 
  #define AM (1.0/IM) 
  #define IQ 127773 
  #define IR 2836 
  #define MASK 123459876 
   
  double ran0(long *idum){ 
    long k; 
    double ans; 
     
    *idum ^= MASK; 
    k=(*idum)/IQ; 
    *idum=IA*(*idum-k*IQ)-IR*k; 
    if (*idum < 0) *idum += IM; 
    ans=AM*(*idum); 
    *idum ^= MASK; 
    return ans; 
  } 
  #undef IA 
  #undef IM 
  #undef AM 
  #undef IQ 
  #undef IR 
  #undef MASK 
  /* (C) Copr. 1986-92 Numerical Recipes Software 9z!+!1(t+%. */ 
}; 
 
#endif