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") ;
}
}
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 ;
}
inline Dart GMap2::phi2(Dart d) const
{
return beta2(beta0(d)) ;
}
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) ;
}
inline bool GMap2::sameEdge(Dart d, Dart e) const
{
return d == e || beta2(d) == e || beta0(d) == e || beta2(beta0(d)) == e ;
}
inline Dart GMap2::alpha0(Dart d) const
{
return beta2(beta0(d)) ;
}
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));
}
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 ;
}
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);
}
}
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*) ®ionCom[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);
}
void GMap0::deleteEdge(Dart d)
{
deleteDart(beta0(d));
deleteDart(d);
}
inline Dart GMap1<MAP_IMPL>::alpha_1(Dart d) const
{
return beta0(beta1(d)) ;
}
inline Dart GMap3::alpha0(Dart d)
{
return beta3(beta0(d)) ;
}
inline Dart GMap3::phi3(Dart d)
{
return beta3(beta0(d)) ;
}