示例#1
0
inline Dart GMap2::beta(const Dart d)
{
	assert( (N > 0) || !"negative parameters not allowed in template multi-beta");
	if (N<10)
	{
		switch(N)
		{
		case 0 : return beta0(d) ;
		case 1 : return beta1(d) ;
		case 2 : return beta2(d) ;
		default : assert(!"Wrong multi-beta relation value") ;
		}
	}
	switch(N%10)
	{
	case 0 : return beta0(beta<N/10>(d)) ;
	case 1 : return beta1(beta<N/10>(d)) ;
	case 2 : return beta2(beta<N/10>(d)) ;
	default : assert(!"Wrong multi-beta relation value") ;
	}
}
示例#2
0
bool EmbeddedGMap2::uncutEdge(Dart d)
{
	if(GMap2::uncutEdge(d))
	{
		if(isOrbitEmbedded<EDGE>())
		{
			unsigned int eEmb = getEmbedding<EDGE>(d) ;
			setDartEmbedding<EDGE>(phi2(d), eEmb) ;
			setDartEmbedding<EDGE>(beta0(d), eEmb) ;
		}
		return true ;
	}
	return false ;
}
示例#3
0
inline Dart GMap2::phi2(Dart d) const
{
	return beta2(beta0(d)) ;
}
示例#4
0
inline bool GMap2::foreach_dart_of_cc(Dart d, FunctorType& f, unsigned int thread) const
{
	return GMap2::foreach_dart_of_oriented_cc(d, f, thread) || GMap2::foreach_dart_of_oriented_cc(beta0(d), f, thread) ;
}
示例#5
0
inline bool GMap2::sameEdge(Dart d, Dart e) const
{
	return d == e || beta2(d) == e || beta0(d) == e || beta2(beta0(d)) == e ;
}
示例#6
0
inline Dart GMap2::alpha0(Dart d) const
{
	return beta2(beta0(d)) ;
}
示例#7
0
RcppExport SEXP nsem3b(SEXP data,  
		      SEXP theta,
		      SEXP Sigma,
		      SEXP modelpar,
		    SEXP control
		    ) {   

  //  srand ( time(NULL) ); /* initialize random seed: */
  
  Rcpp::NumericVector Theta(theta);  
  Rcpp::NumericMatrix D(data);
  unsigned nobs = D.nrow(), k = D.ncol();
  mat Data(D.begin(), nobs, k, false); // Avoid copying
  Rcpp::NumericMatrix V(Sigma);  
  mat S(V.begin(), V.nrow(), V.ncol()); 
  S(0,0) = 1;
  mat iS = inv(S);
  double detS = det(S);
 

  Rcpp::List Modelpar(modelpar);
  // Rcpp::IntegerVector _nlatent = Modelpar["nlatent"]; unsigned nlatent = _nlatent[0];
  Rcpp::IntegerVector _ny0 = Modelpar["nvar0"]; unsigned ny0 = _ny0[0];
  Rcpp::IntegerVector _ny1 = Modelpar["nvar1"]; unsigned ny1 = _ny1[0];
  Rcpp::IntegerVector _ny2 = Modelpar["nvar2"]; unsigned ny2 = _ny2[0];
  Rcpp::IntegerVector _npred0 = Modelpar["npred0"]; unsigned npred0 = _npred0[0];
  Rcpp::IntegerVector _npred1 = Modelpar["npred1"]; unsigned npred1 = _npred1[0];
  Rcpp::IntegerVector _npred2 = Modelpar["npred2"]; unsigned npred2 = _npred2[0];
  Rcpp::List Control(control);   
  Rcpp::NumericVector _lambda = Control["lambda"]; double lambda = _lambda[0];
  Rcpp::NumericVector _niter = Control["niter"]; double niter = _niter[0];
  Rcpp::NumericVector _Dtol = Control["Dtol"]; double Dtol = _Dtol[0];


  rowvec mu0(ny0), lambda0(ny0);
  rowvec mu1(ny1), lambda1(ny1);
  rowvec mu2(ny2), lambda2(ny2);
  rowvec beta0(npred0); 
  rowvec beta1(npred1); 
  rowvec beta2(npred2);
  rowvec gamma(2);
  rowvec gamma2(2);  
  unsigned pos=0;
  for (unsigned i=0; i<ny0; i++) {
    mu0(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<ny1; i++) {
    mu1(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<ny2; i++) {
    mu2(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<ny0; i++) {
    lambda0(i) = Theta[pos];
    pos++;
  }
  lambda1(0) = 1;
  for (unsigned i=1; i<ny1; i++) {
    lambda1(i) = Theta[pos];
    pos++;
  }
  lambda2(0) = 1;
  for (unsigned i=1; i<ny2; i++) {
    lambda2(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<npred0; i++) {
    beta0(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<npred1; i++) {
    beta1(i) = Theta[pos];
    pos++;
  }
  for (unsigned i=0; i<npred2; i++) {
    beta2(i) = Theta[pos];
    pos++;
  }
  gamma(0) = Theta[pos]; gamma(1) = Theta[pos+1];
  gamma2(0) = Theta[pos+2]; gamma2(1) = Theta[pos+3];

  // cerr << "mu0=" << mu0 << endl;
  // cerr << "mu1=" << mu1 << endl;
  // cerr << "mu2=" << mu2 << endl;
  // cerr << "lambda0=" << lambda0 << endl;
  // cerr << "lambda1=" << lambda1 << endl;
  // cerr << "lambda2=" << lambda2 << endl;
  // cerr << "beta0=" << beta0 << endl;
  // cerr << "beta1=" << beta1 << endl;
  // cerr << "beta2=" << beta2 << endl;
  // cerr << "gamma=" << gamma << endl;
  // cerr << "gamma2=" << gamma2 << endl;
  
  mat lap(nobs,4);
  for (unsigned i=0; i<nobs; i++) {
    rowvec newlap = laNRb(Data.row(i), iS, detS,
			  mu0, mu1, mu2, 
			  lambda0, lambda1, lambda2, 
			  beta0,beta1, beta2, gamma, gamma2,
			  Dtol,niter,lambda);
    lap.row(i) = newlap;
  }

  List  res;
  res["indiv"] = lap;
  res["logLik"] = sum(lap.col(0)) + (3-V.nrow())*log(2.0*datum::pi)*nobs/2;
  res["norm0"] = (3-V.nrow())*log(2*datum::pi)/2;
  return res;
}
// Coordinate descent for logistic models (no active set cycling)
RcppExport SEXP cdfit_binomial_hsr_slores_nac(SEXP X_, SEXP y_, SEXP n_pos_, SEXP ylab_, 
                                          SEXP row_idx_, SEXP lambda_, SEXP nlambda_,
                                          SEXP lam_scale_, SEXP lambda_min_, 
                                          SEXP alpha_, SEXP user_, 
                                          SEXP eps_, SEXP max_iter_, SEXP multiplier_, 
                                          SEXP dfmax_, SEXP ncore_, SEXP warn_,
                                          SEXP safe_thresh_, SEXP verbose_) {
  XPtr<BigMatrix> xMat(X_);
  double *y = REAL(y_);
  int n_pos = INTEGER(n_pos_)[0];
  IntegerVector ylabel = Rcpp::as<IntegerVector>(ylab_); // label vector of {-1, 1}
  int *row_idx = INTEGER(row_idx_);
  double lambda_min = REAL(lambda_min_)[0];
  double alpha = REAL(alpha_)[0];
  int n = Rf_length(row_idx_); // number of observations used for fitting model
  int p = xMat->ncol();
  int L = INTEGER(nlambda_)[0];
  int lam_scale = INTEGER(lam_scale_)[0];
  double eps = REAL(eps_)[0];
  int max_iter = INTEGER(max_iter_)[0];
  double *m = REAL(multiplier_);
  int dfmax = INTEGER(dfmax_)[0];
  int warn = INTEGER(warn_)[0];
  int user = INTEGER(user_)[0];
  double slores_thresh = REAL(safe_thresh_)[0]; // threshold for safe test
  int verbose = INTEGER(verbose_)[0];

  NumericVector lambda(L);
  NumericVector Dev(L);
  IntegerVector iter(L);
  IntegerVector n_reject(L); // number of total rejections;
  IntegerVector n_slores_reject(L); // number of safe rejections;
  NumericVector beta0(L);
  NumericVector center(p);
  NumericVector scale(p);
  int p_keep = 0; // keep columns whose scale > 1e-6
  int *p_keep_ptr = &p_keep;
  vector<int> col_idx;
  vector<double> z;
  double lambda_max = 0.0;
  double *lambda_max_ptr = &lambda_max;
  int xmax_idx = 0;
  int *xmax_ptr = &xmax_idx;
  
  // set up omp
  int useCores = INTEGER(ncore_)[0];
#ifdef BIGLASSO_OMP_H_
  int haveCores = omp_get_num_procs();
  if(useCores < 1) {
    useCores = haveCores;
  }
  omp_set_dynamic(0);
  omp_set_num_threads(useCores);
#endif
  
  if (verbose) {
    char buff1[100];
    time_t now1 = time (0);
    strftime (buff1, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now1));
    Rprintf("\nPreprocessing start: %s\n", buff1);
  }
  
  // standardize: get center, scale; get p_keep_ptr, col_idx; get z, lambda_max, xmax_idx;
  standardize_and_get_residual(center, scale, p_keep_ptr, col_idx, z, lambda_max_ptr, xmax_ptr, xMat, 
                               y, row_idx, lambda_min, alpha, n, p);
  p = p_keep; // set p = p_keep, only loop over columns whose scale > 1e-6

  if (verbose) {
    char buff1[100];
    time_t now1 = time (0);
    strftime (buff1, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now1));
    Rprintf("Preprocessing end: %s\n", buff1);
    Rprintf("\n-----------------------------------------------\n");
  }

  arma::sp_mat beta = arma::sp_mat(p, L); //beta
  double *a = Calloc(p, double); //Beta from previous iteration
  double a0 = 0.0; //beta0 from previousiteration
  double *w = Calloc(n, double);
  double *s = Calloc(n, double); //y_i - pi_i
  double *eta = Calloc(n, double);
  // int *e1 = Calloc(p, int); //ever-active set
  int *e2 = Calloc(p, int); //strong set
  double xwr, xwx, pi, u, v, cutoff, l1, l2, shift, si;
  double max_update, update, thresh; // for convergence check
  int i, j, jj, l, violations, lstart;
  
  double ybar = sum(y, n) / n;
  a0 = beta0[0] = log(ybar / (1-ybar));
  double nullDev = 0;
  double *r = Calloc(n, double);
  for (i = 0; i < n; i++) {
    r[i] = y[i];
    nullDev = nullDev - y[i]*log(ybar) - (1-y[i])*log(1-ybar);
    s[i] = y[i] - ybar;
    eta[i] = a0;
  }
  thresh = eps * nullDev / n;
  double sumS = sum(s, n); // temp result sum of s
  double sumWResid = 0.0; // temp result: sum of w * r

  // set up lambda
  if (user == 0) {
    if (lam_scale) { // set up lambda, equally spaced on log scale
      double log_lambda_max = log(lambda_max);
      double log_lambda_min = log(lambda_min*lambda_max);
      
      double delta = (log_lambda_max - log_lambda_min) / (L-1);
      for (l = 0; l < L; l++) {
        lambda[l] = exp(log_lambda_max - l * delta);
      }
    } else { // equally spaced on linear scale
      double delta = (lambda_max - lambda_min*lambda_max) / (L-1);
      for (l = 0; l < L; l++) {
        lambda[l] = lambda_max - l * delta;
      }
    }
    Dev[0] = nullDev;
    lstart = 1;
    n_reject[0] = p;
  } else {
    lstart = 0;
    lambda = Rcpp::as<NumericVector>(lambda_);
  }

  // Slores variables
  vector<double> theta_lam;
  double g_theta_lam = 0.0;
  double prod_deriv_theta_lam = 0.0;
  double *g_theta_lam_ptr = &g_theta_lam;
  double *prod_deriv_theta_lam_ptr = &prod_deriv_theta_lam;
  vector<double> X_theta_lam_xi_pos; 
  vector<double> prod_PX_Pxmax_xi_pos;
  vector<double> cutoff_xi_pos;
  int *slores_reject = Calloc(p, int);
  int *slores_reject_old = Calloc(p, int);
  for (int j = 0; j < p; j++) slores_reject_old[j] = 1;
  
  int slores; // if 0, don't perform Slores rule
  if (slores_thresh < 1) {
    slores = 1; // turn on slores
    theta_lam.resize(n);
    X_theta_lam_xi_pos.resize(p);
    prod_PX_Pxmax_xi_pos.resize(p);
    cutoff_xi_pos.resize(p);

    slores_init(theta_lam, g_theta_lam_ptr, prod_deriv_theta_lam_ptr, cutoff_xi_pos,
                X_theta_lam_xi_pos, prod_PX_Pxmax_xi_pos, 
                xMat, y, z, xmax_idx, row_idx, col_idx, 
                center, scale, ylabel, n_pos, n, p);
  } else {
    slores = 0;
  }
  
  if (slores == 1 && user == 0) n_slores_reject[0] = p;

  for (l = lstart; l < L; l++) {
    if(verbose) {
      // output time
      char buff[100];
      time_t now = time (0);
      strftime (buff, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now));
      Rprintf("Lambda %d. Now time: %s\n", l, buff);
    }
    
    if (l != 0) {
      // Check dfmax
      int nv = 0;
      for (j = 0; j < p; j++) {
        if (a[j] != 0) {
          nv++;
        }
      }
      if (nv > dfmax) {
        for (int ll=l; ll<L; ll++) iter[ll] = NA_INTEGER;
        Free(slores_reject);
        Free(slores_reject_old);
        Free_memo_bin_hsr_nac(s, w, a, r, e2, eta);
        return List::create(beta0, beta, center, scale, lambda, Dev, 
                            iter, n_reject, Rcpp::wrap(col_idx));
      }
      cutoff = 2*lambda[l] - lambda[l-1];
    } else {
      cutoff = 2*lambda[l] - lambda_max;
    }

    if (slores) {
      slores_screen(slores_reject, theta_lam, g_theta_lam, prod_deriv_theta_lam,
                    X_theta_lam_xi_pos, prod_PX_Pxmax_xi_pos, cutoff_xi_pos,
                    row_idx, col_idx, center, scale, xmax_idx, ylabel, 
                    lambda[l], lambda_max, n_pos, n, p);
      n_slores_reject[l] = sum(slores_reject, p);
      
      // update z[j] for features which are rejected at previous lambda but accepted at current one.
      update_zj(z, slores_reject, slores_reject_old, xMat, row_idx, col_idx, center, scale, sumS, s, m, n, p);
      
      #pragma omp parallel for private(j) schedule(static) 
      for (j = 0; j < p; j++) {
        slores_reject_old[j] = slores_reject[j];
        // hsr screening
        // if (slores_reject[j] == 0 && (fabs(z[j]) > (cutoff * alpha * m[col_idx[j]]))) {
        if (fabs(z[j]) > (cutoff * alpha * m[col_idx[j]])) {
          e2[j] = 1;
        } else {
          e2[j] = 0;
        }
      }
    } else {
      n_slores_reject[l] = 0; 
      // hsr screening over all
      #pragma omp parallel for private(j) schedule(static) 
      for (j = 0; j < p; j++) {
        if (fabs(z[j]) > (cutoff * alpha * m[col_idx[j]])) {
          e2[j] = 1;
        } else {
          e2[j] = 0;
        }
      }
    }
    n_reject[l] = p - sum(e2, p);
    
    while (iter[l] < max_iter) {
      while (iter[l] < max_iter) {
        while (iter[l] < max_iter) {
          iter[l]++;
          Dev[l] = 0.0;
          
          for (i = 0; i < n; i++) {
            if (eta[i] > 10) {
              pi = 1;
              w[i] = .0001;
            } else if (eta[i] < -10) {
              pi = 0;
              w[i] = .0001;
            } else {
              pi = exp(eta[i]) / (1 + exp(eta[i]));
              w[i] = pi * (1 - pi);
            }
            s[i] = y[i] - pi;
            r[i] = s[i] / w[i];
            if (y[i] == 1) {
              Dev[l] = Dev[l] - log(pi);
            } else {
              Dev[l] = Dev[l] - log(1-pi);
            }
          }
          
          if (Dev[l] / nullDev < .01) {
            if (warn) warning("Model saturated; exiting...");
            for (int ll=l; ll<L; ll++) iter[ll] = NA_INTEGER;
            Free(slores_reject);
            Free(slores_reject_old);
            Free_memo_bin_hsr_nac(s, w, a, r, e2, eta);
            return List::create(beta0, beta, center, scale, lambda, Dev, iter, n_reject, n_slores_reject, Rcpp::wrap(col_idx));
          }
          // Intercept
          xwr = crossprod(w, r, n, 0);
          xwx = sum(w, n);
          beta0[l] = xwr / xwx + a0;
          si = beta0[l] - a0;
          if (si != 0) {
            a0 = beta0[l];
            for (i = 0; i < n; i++) {
              r[i] -= si; //update r
              eta[i] += si; //update eta
            }
          }
          sumWResid = wsum(r, w, n); // update temp result: sum of w * r, used for computing xwr;
          max_update = 0.0;
          for (j = 0; j < p; j++) {
            if (e2[j]) {
              jj = col_idx[j];
              xwr = wcrossprod_resid(xMat, r, sumWResid, row_idx, center[jj], scale[jj], w, n, jj);
              v = wsqsum_bm(xMat, w, row_idx, center[jj], scale[jj], n, jj) / n;
              u = xwr/n + v * a[j];
              l1 = lambda[l] * m[jj] * alpha;
              l2 = lambda[l] * m[jj] * (1-alpha);
              beta(j, l) = lasso(u, l1, l2, v);

              shift = beta(j, l) - a[j];
              if (shift != 0) {
                // update change of objective function
                // update = - u * shift + (0.5 * v + 0.5 * l2) * (pow(beta(j, l), 2) - pow(a[j], 2)) + l1 * (fabs(beta(j, l)) - fabs(a[j]));
                update = pow(beta(j, l) - a[j], 2) * v;
                if (update > max_update) max_update = update;
                update_resid_eta(r, eta, xMat, shift, row_idx, center[jj], scale[jj], n, jj); // update r
                sumWResid = wsum(r, w, n); // update temp result w * r, used for computing xwr;
                a[j] = beta(j, l); // update a
              }
            }
          }
          // Check for convergence
          if (max_update < thresh)  break;
        }

      }
      // Scan for violations in rest
      if (slores) {
        violations = check_rest_set_hsr_slores_nac(e2, slores_reject, z, xMat, row_idx, col_idx, center, scale, a, lambda[l], sumS, alpha, s, m, n, p);
      } else {
        violations = check_rest_set_bin_nac(e2, z, xMat, row_idx, col_idx, center, scale, a, lambda[l], sumS, alpha, s, m, n, p);
      }
      if (violations == 0) break;
      if (n_slores_reject[l] <= p * slores_thresh) {
        slores = 0; // turn off slores screening for next iteration if not efficient
      }
    }
  }
  Free(slores_reject);
  Free(slores_reject_old);
  Free_memo_bin_hsr_nac(s, w, a, r, e2, eta);
  return List::create(beta0, beta, center, scale, lambda, Dev, iter, n_reject, n_slores_reject, Rcpp::wrap(col_idx));
}
示例#9
0
bool EmbeddedGMap3::check()
{
    bool topo = GMap3::check() ;
    if (!topo)
        return false ;

    CGoGNout << "Check: embedding begin" << CGoGNendl ;

    for(Dart d = begin(); d != end(); next(d))
    {
        if(isOrbitEmbedded<VERTEX>())
        {
            if( getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta1(d)) ||
                    getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta2(d)) ||
                    getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta3(d)) )
            {
                std::cout << "Embedding Check : different embeddings on vertex" << std::endl ;
                return false ;
            }
        }

        if(isOrbitEmbedded<EDGE>())
        {
            if( getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta0(d)) ||
                    getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta2(d)) ||
                    getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta3(d)) )
            {
                std::cout << "Embedding Check : different embeddings on edge" << std::endl ;
                return false ;
            }
        }

        if (isOrbitEmbedded<FACE>())
        {
            if( getEmbedding<FACE>(d) != getEmbedding<FACE>(beta0(d)) ||
                    getEmbedding<FACE>(d) != getEmbedding<FACE>(beta1(d)) ||
                    getEmbedding<FACE>(d) != getEmbedding<FACE>(beta3(d)) )
            {
                CGoGNout << "Check: different embeddings on face" << CGoGNendl ;
                return false ;
            }
        }

        if (isOrbitEmbedded<VOLUME>())
        {
            if( getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta0(d)) ||
                    getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta1(d)) ||
                    getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta2(d)) )
            {
                CGoGNout << "Check: different embeddings on volume" << CGoGNendl ;
                return false ;
            }
        }
    }

    CGoGNout << "Check: embedding ok" << CGoGNendl ;

    std::cout << "nb vertex orbits : " << getNbOrbits<VERTEX>() << std::endl ;
    std::cout << "nb vertex cells : " << m_attribs[VERTEX].size() << std::endl ;

    std::cout << "nb edge orbits : " << getNbOrbits<EDGE>() << std::endl ;
    std::cout << "nb edge cells : " << m_attribs[EDGE].size() << std::endl ;

    std::cout << "nb face orbits : " << getNbOrbits<FACE>() << std::endl ;
    std::cout << "nb face cells : " << m_attribs[FACE].size() << std::endl ;

    std::cout << "nb volume orbits : " << getNbOrbits<VOLUME>() << std::endl ;
    std::cout << "nb volume cells : " << m_attribs[VOLUME].size() << std::endl ;

    return true ;
}
示例#10
0
void EmbeddedGMap3::unsewVolumes(Dart d)
{
    Dart dd = alpha1(d);

    unsigned int fEmb = EMBNULL ;
    if(isOrbitEmbedded<FACE>())
        fEmb = getEmbedding<FACE>(d) ;

    GMap3::unsewVolumes(d);

    Dart dit = d;
    do
    {
        // embed the unsewn vertex orbit with the vertex embedding if it is deconnected
        if(isOrbitEmbedded<VERTEX>())
        {
            if(!sameVertex(dit, dd))
            {
                setOrbitEmbedding<VERTEX>(dit, getEmbedding<VERTEX>(dit)) ;
                setOrbitEmbeddingOnNewCell<VERTEX>(dd);
                copyCell<VERTEX>(dd, dit);
            }
            else
            {
                setOrbitEmbedding<VERTEX>(dit, getEmbedding<VERTEX>(dit)) ;
            }
        }

        dd = phi_1(dd);

        // embed the unsewn edge with the edge embedding if it is deconnected
        if(isOrbitEmbedded<EDGE>())
        {
            if(!sameEdge(dit, dd))
            {
                setOrbitEmbedding<EDGE>(dit, getEmbedding<EDGE>(dit)) ;
                setOrbitEmbeddingOnNewCell<EDGE>(dd);
                copyCell<EDGE>(dd, dit);
            }
            else
            {
                setOrbitEmbedding<EDGE>(dit, getEmbedding<EDGE>(dit)) ;
            }
        }

        if(isOrbitEmbedded<FACE>())
        {
            setDartEmbedding<FACE>(beta3(dit), fEmb) ;
            setDartEmbedding<FACE>(beta0(beta3(dit)), fEmb) ;
        }

        dit = phi1(dit);
    } while(dit != d);

    // embed the unsewn face with the face embedding
    if (isOrbitEmbedded<FACE>())
    {
        setOrbitEmbeddingOnNewCell<FACE>(dd);
        copyCell<FACE>(dd, d);
    }
}
示例#11
0
void EmbeddedGMap3::splitFace(Dart d, Dart e)
{
    Dart dd = beta1(beta3(d));
    Dart ee = beta1(beta3(e));

    GMap3::splitFace(d, e);

    if(isOrbitEmbedded<VERTEX>())
    {
        unsigned int vEmb1 = getEmbedding<VERTEX>(d) ;
        unsigned int vEmb2 = getEmbedding<VERTEX>(e) ;

        setDartEmbedding<VERTEX>(beta1(d), vEmb1);
        setDartEmbedding<VERTEX>(beta2(beta1(d)), vEmb1);
        setDartEmbedding<VERTEX>(beta1(beta2(beta1(d))), vEmb1);
        setDartEmbedding<VERTEX>(beta1(dd), vEmb1);
        setDartEmbedding<VERTEX>(beta2(beta1(dd)), vEmb1);
        setDartEmbedding<VERTEX>(beta1(beta2(beta1(dd))), vEmb1);

        setDartEmbedding<VERTEX>(beta1(e), vEmb2);
        setDartEmbedding<VERTEX>(beta2(beta1(e)), vEmb2);
        setDartEmbedding<VERTEX>(beta1(beta2(beta1(e))), vEmb2);
        setDartEmbedding<VERTEX>(beta1(ee), vEmb2);
        setDartEmbedding<VERTEX>(beta2(beta1(ee)), vEmb2);
        setDartEmbedding<VERTEX>(beta1(beta2(beta1(ee))), vEmb2);
    }

    if(isOrbitEmbedded<EDGE>())
    {
        setOrbitEmbedding<EDGE>(beta1(d), getEmbedding<EDGE>(beta1(d))) ;
        setOrbitEmbedding<EDGE>(beta1(e), getEmbedding<EDGE>(beta1(e))) ;

        setOrbitEmbedding<EDGE>(d, getEmbedding<EDGE>(d)) ;
        setOrbitEmbedding<EDGE>(e, getEmbedding<EDGE>(e)) ;
        setOrbitEmbedding<EDGE>(beta1(beta2(beta1(d))), getEmbedding<EDGE>(beta1(beta2(beta1(d))))) ;
        setOrbitEmbedding<EDGE>(beta1(beta2(beta1(e))), getEmbedding<EDGE>(beta1(beta2(beta1(e))))) ;
    }


    if(isOrbitEmbedded<FACE>())
    {
        unsigned int fEmb = getEmbedding<FACE>(d) ;
        setDartEmbedding<FACE>(beta1(d), fEmb) ;
        setDartEmbedding<FACE>(beta0(beta1(d)), fEmb) ;
        setDartEmbedding<FACE>(beta1(beta0(beta1(d))), fEmb) ;
        setDartEmbedding<FACE>(beta1(ee), fEmb) ;
        setDartEmbedding<FACE>(beta0(beta1(ee)), fEmb) ;
        setDartEmbedding<FACE>(beta1(beta0(beta1(ee))), fEmb) ;
        setOrbitEmbeddingOnNewCell<FACE>(e);
        copyCell<FACE>(e, d);
    }

    if(isOrbitEmbedded<VOLUME>())
    {
        unsigned int vEmb1 = getEmbedding<VOLUME>(d) ;
        setDartEmbedding<VOLUME>(beta1(d),  vEmb1);
        setDartEmbedding<VOLUME>(beta0(beta1(d)),  vEmb1);
        setDartEmbedding<VOLUME>(beta1(beta0(beta1(d))), vEmb1) ;
        setDartEmbedding<VOLUME>(beta1(e),  vEmb1);
        setDartEmbedding<VOLUME>(beta0(beta1(e)),  vEmb1);
        setDartEmbedding<VOLUME>(beta1(beta0(beta1(e))), vEmb1) ;

        unsigned int vEmb2 = getEmbedding<VOLUME>(dd) ;
        setDartEmbedding<VOLUME>(beta1(dd),  vEmb2);
        setDartEmbedding<VOLUME>(beta0(beta1(dd)),  vEmb2);
        setDartEmbedding<VOLUME>(beta1(beta0(beta1(dd))), vEmb2) ;
        setDartEmbedding<VOLUME>(beta1(ee),  vEmb2);
        setDartEmbedding<VOLUME>(beta0(beta1(ee)),  vEmb2);
        setDartEmbedding<VOLUME>(beta1(beta0(beta1(ee))), vEmb2) ;
    }
}
void dgCollisionDeformableSolidMesh::ResolvePositionsConstraints (dgFloat32 timestep)
{
	dgAssert (m_myBody);

	dgInt32 strideInBytes = sizeof (dgVector) * m_clustersCount + sizeof (dgMatrix) * m_clustersCount + sizeof (dgMatrix) * m_particles.m_count;
	m_world->m_solverMatrixMemory.ExpandCapacityIfNeessesary (1, strideInBytes);
	dgVector* const regionCom = (dgVector*)&m_world->m_solverMatrixMemory[0];
	dgMatrix* const sumQiPi = (dgMatrix*) &regionCom[m_clustersCount];
	dgMatrix* const covarianceMatrix = (dgMatrix*) &sumQiPi[m_clustersCount];

	const dgFloat32* const masses = m_particles.m_unitMass;
	dgVector zero (dgFloat32 (0.0f));
	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		regionCom[i] = zero;
	}

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		dgVector mass (masses[i]);
		const dgVector& r = m_posit[i];
		const dgVector& r0 = m_shapePosit[i];
		dgVector mr (r.Scale4(masses[i]));
		covarianceMatrix[i] = dgMatrix (r0, mr);
	
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			regionCom[index] += mr;
		}
	}

	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		dgVector mcr (regionCom[i]);
		regionCom[i] = mcr.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]);
		const dgVector& cr0 = m_clusterCom0[i];
		sumQiPi[i] = dgMatrix (cr0, mcr.CompProduct4(dgVector::m_negOne));
	}

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;
		const dgMatrix& covariance = covarianceMatrix[i];
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			dgMatrix& QiPi = sumQiPi[index];
			QiPi.m_front += covariance.m_front;
			QiPi.m_up += covariance.m_up;
			QiPi.m_right += covariance.m_right;
		}
	}

dgVector beta0 (dgFloat32 (0.93f));
//dgVector beta0 (dgFloat32 (0.0f));
dgVector beta1 (dgVector::m_one - beta0);
	dgFloat32 stiffness = dgFloat32 (0.3f);

	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		dgMatrix& QiPi = sumQiPi[i];

		dgMatrix S (QiPi * QiPi.Transpose4X4());
		dgVector eigenValues;

		S.EigenVectors (eigenValues, m_clusterRotationInitialGuess[i]);
		m_clusterRotationInitialGuess[i] = S;

#ifdef _DEBUG_____ 
		dgMatrix P0 (QiPi * QiPi.Transpose4X4());
		dgMatrix D (dgGetIdentityMatrix());
		D[0][0] = eigenValues[0];
		D[1][1] = eigenValues[1];
		D[2][2] = eigenValues[2];
		dgMatrix P1 (S.Transpose4X4() * D * S);
		dgAssert (P0.TestSymetric3x3());
		dgAssert (P1.TestSymetric3x3());

		dgMatrix xx (P1 * P0.Symetric3by3Inverse());
		dgAssert (dgAbsf (xx[0][0] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][1] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][2] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[0][1]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[0][2]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][0]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][2]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][0]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][1]) < dgFloat32 (1.0e-3f));
#endif

		eigenValues = eigenValues.InvSqrt();

		dgMatrix m;
		m.m_front = S.m_front.CompProduct4(eigenValues.BroadcastX());
		m.m_up    = S.m_up.CompProduct4(eigenValues.BroadcastY());
		m.m_right = S.m_right.CompProduct4(eigenValues.BroadcastZ());
		m.m_posit = dgVector::m_wOne;
		dgMatrix invS (S.Transpose4X4() * m);
		dgMatrix R (invS * QiPi);
		dgMatrix A (m_clusterAqqInv[i] * QiPi);

		QiPi.m_front = A.m_front.CompProduct4(beta0) + R.m_front.CompProduct4(beta1);
		QiPi.m_up = A.m_up.CompProduct4(beta0) + R.m_up.CompProduct4(beta1);
		QiPi.m_right = A.m_right.CompProduct4(beta0) + R.m_right.CompProduct4(beta1);
	}

	dgVector invTimeScale (stiffness / timestep);
	dgVector* const veloc = m_particles.m_veloc;

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;

		dgVector v (dgFloat32 (0.0f));
		const dgVector& p = m_posit[i];
		const dgVector& p0 = m_shapePosit[i];
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			const dgMatrix& matrix = sumQiPi[index];
			dgVector gi (matrix.RotateVector(p0 - m_clusterCom0[index]) + regionCom[index]);
			v += ((gi - p).CompProduct4(invTimeScale).Scale4 (m_clusterWeight[index]));
		}
		veloc[i] += v;
	}


// resolve collisions here
//for now just a hack a collision plane until I get the engine up an running
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
	dgVector p (m_basePosit + m_posit[i].m_y);
	if (p.m_y < dgFloat32 (0.0f)) {
		m_posit[i].m_y = -m_basePosit.m_y;
		veloc[i].m_y = dgFloat32 (0.0f);
	}
}

	dgVector time (timestep);
	dgVector minBase(dgFloat32 (1.0e10f));
    dgVector minBox (dgFloat32 (1.0e10f));
    dgVector maxBox (dgFloat32 (-1.0e10f));
    dgMatrix matrix (m_myBody->GetCollision()->GetGlobalMatrix().Inverse());
	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		m_posit[i] += veloc[i].CompProduct4 (time);
        m_particles.m_posit[i] = matrix.TransformVector(m_posit[i] + m_basePosit);
		minBase = minBase.GetMin (m_posit[i]);
        minBox = minBox.GetMin(m_particles.m_posit[i]);
        maxBox = maxBox.GetMax(m_particles.m_posit[i]);
	}

	minBase = minBase.Floor();
	dgVector mask ((minBase < dgVector (dgFloat32 (0.0f))) | (minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))));
	dgInt32 test = mask.GetSignMask();
	if (test & 0x07) {
		dgVector offset (((minBase < dgVector (dgFloat32 (0.0f))) & dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE/2))) + 
			             ((minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))) & dgVector (dgFloat32 (-DG_SOFTBODY_BASE_SIZE/2))));
		m_basePosit -= offset;
		for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
			m_posit[i] += offset;
		}
	}

	// integrate each particle by the deformation velocity, also calculate the new com
	// calculate the new body average velocity
//	if (m_myBody->m_matrixUpdate) {
//		myBody->m_matrixUpdate (*myBody, myBody->m_matrix, threadIndex);
//	}
	// the collision changed shape, need to update spatial structure 
//	UpdateCollision ();
//	SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
    SetCollisionBBox (minBox, maxBox);
}
示例#13
0
文件: gmap0.cpp 项目: mhdsedighi/SOFA
void GMap0::deleteEdge(Dart d)
{
	deleteDart(beta0(d));
	deleteDart(d);
}
示例#14
0
文件: gmap1.hpp 项目: Peiffert/CGoGN
inline Dart GMap1<MAP_IMPL>::alpha_1(Dart d) const
{
	return beta0(beta1(d)) ;
}
示例#15
0
文件: gmap3.hpp 项目: mhdsedighi/SOFA
inline Dart GMap3::alpha0(Dart d)
{
	return beta3(beta0(d)) ;
}
示例#16
0
文件: gmap3.hpp 项目: mhdsedighi/SOFA
inline Dart GMap3::phi3(Dart d)
{
	return beta3(beta0(d)) ;
}